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I. 


INTRODUCTION 


A.  NPS  BACKGROUND 

The  Naval  Postgraduate  School  Small  Robot  Initiative  (SMART)  is  an  ongoing 
effort  of  the  Combat  Systems  Science  and  Technology  Department  to  develop 
autonomous  robots  with  potential  military  applications.  Utilizing  Commercial  Off-the- 
Shelf  (COTS)  components,  this  program  seeks  to  design,  build,  and  operate  cost  effective 
robots  that  are  highly  mobile,  can  perform  waypoint  navigation  and  dead  reckoning, 
conduct  obstacle  avoidance,  and  support  a  variety  of  different  mechanisms  to  perceive 
and  interact  with  their  environment. 

There  is  significant  interest  in  developing  robots  for  operation  in  rugged, 
unstructured  environments.  In  particular,  the  ability  to  deploy  autonomous  robots  in 
beach,  surf-zone,  and  near-shore  environments  is  highly  sought  after  for  both  civilian  and 
military  applications.  Potential  applications  include  the  ability  to  conduct  coastal  surveys, 
covert  surveillance,  mine  identification,  and  mine  clearance  operations.  However,  this 
environment  is  extraordinarily  challenging  for  a  robot  to  operate  in  and  no  current  robotic 
applications  have  been  successfully  fielded.  The  robot  would  need  to  be  robust  enough  to 
navigate  a  varied  and  changing  environment  to  include  soft  and  shifting  sands,  the 
movement  of  the  tides  and  waves,  and  an  uneven  surface  with  few  level  areas.  In  addition 
to  this,  the  robot  would  need  to  be  rugged  enough  to  withstand  water  to  a  certain  depth, 
salt  water  corrosion,  and  various  kinetic  stresses  from  operating  in  a  real  world 
environment. 

B.  WHEGS  PLATFORM  DEVELOPMENT 

1.  Biologically  Inspired  Movement 

Whegs™  (wheel-legs)  is  a  class  of  robot  developed  using  basic  mechanical 
designs  that  were  inspired  by  the  locomotive  principles  of  biological  organisms.  Roger 
Quinn  of  Case  Western  Reserve  University  developed  this  method  for  utilization  in  the 
university’s  Biologically  Inspired  Robotics  Laboratory.  The  motion  of  Whegs™  concept 
was  inspired  by  the  mobility  of  hexapods,  specifically  the  cockroach,  to  take  advantage 
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of  the  insect’s  inherent  stability  and  mobility  over  a  variety  of  terrain.  The  principals  of 
cockroach  motion  are  that  it  stands  and  moves  using  six  legs.  It  walks  using  a  tripod  gait, 
wherein  the  front  and  rear  legs  of  the  body  move  in  phase  with  the  middle  leg  of  the 
opposite  side  of  the  body.  When  it  encounters  a  large  obstacle,  its  gait  changes  by 
engaging  all  of  its  Whegs™  simultaneously  to  surmount  the  obstacle  [1].  It  also  flexes  its 
body  joint  to  permit  greater  vertical  reach  for  its  legs  and  prevents  high  centering  off  the 
obstacle  during  a  climb  by  bending  the  front  half  of  its  body  down. 

2.  Early  WHEGS™  Designs 

Whegs™  utilize  a  unique  wheel-leg  design  to  take  advantage  of  both  the 
enhanced  mobility  of  legged  platforms  and  the  simplicity  of  wheel  driven  robots.  The 
Whegs™  is  a  simple  design  that  utilizes  COTS  resources  to  greatly  increase  mobility 
over  normal  wheeled  robots  without  the  need  for  complicated  low  level  control 
algorithms.  The  standard  Whegs™  wheel  consists  of  symmetrically  spaced  spokes 
attached  to  a  central  hub.  Passive  mechanical  adaptation  is  incorporated  through  the  use 
of  torsional  compliance  devices  in  the  axles,  permitting  the  opposing  Whegs™  to  engage 
for  additional  torque  to  surmount  significant  obstacles.  After  the  obstacle  has  been 
surmounted,  the  opposing  Whegs™  will  automatically  return  to  its  normal  gait  for 
continued  translation. 

The  previous  Whegs™  design  used  by  the  NPS  SMART  program  was  the  Dayton 
Area  Graduate  Studies  Institute  (DAGSI)  Whegs™  prototype  called  Agbot.  The  Agbot 
platform  was  built  as  a  collaboration  between  NPS  and  Case  Western  Reserve  University 
Biologically  Inspired  Robots  Laboratory.  Agbot,  pictured  in  Figure  1,  is  a  six-legged 
robot  with  a  tripod  gait.  It  utilizes  one  drive  motor  to  move  the  Whegs™,  which  are 
mechanically  linked  through  a  chain  and  gear  system.  Passive  compliance  on  Agbot  is 
performed  by  a  limited  slip  differential  which  consists  of  two  coaxial  axles  linked  by  a 
spring.  Steering  is  accomplished  by  turning  the  front  and  back  leg  sets  inboard  in 
opposition,  accounting  for  a  large  turning  radius.  Agbot  did  not  have  body  joint  flexion 
incorporated,  but  the  chasis  was  built  in  two  halves  with  a  body  joint.  An  in-depth  review 
of  Agbot  can  be  found  in  [2], 
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Figure  1.  Agbot  (From  [2]) 


3.  Pelican  Whegs™  Development 

The  Pelican  Whegs  is  the  latest  design  effort  under  this  initiative.  Alexander 
Boxerbaum  of  Case  Western  Reserve  University  is  building  the  Pelican  Whegs  body  as 
part  of  the  ongoing  examination  of  Whegs-based  robotic  applications. 

The  Pelican  Whegs™  platform  replaces  the  hexapod  locomotion  and  body  joint 
flexion  of  the  DAGSI  Whegs™  with  quadruped  locomotion  and  a  motorized  tail  as  seen 
in  Figure  2.  This  robot  moves  using  a  diagonal  gait  instead  of  the  more  stable  tripod  gait 
of  the  earlier  model.  To  improve  stability,  the  three  spoke  wheel-legs  have  been  replaced 
by  four  evenly  spaced  spoke  wheel-legs  to  reduce  body  roll  and  vertical  translation 
during  motion.  The  right  and  left  side  Whegs™  are  driven  using  separate  drive  motors, 
and  each  set  is  linked  mechanically.  This  permits  Pelican  Whegs™  to  take  advantage  of 
the  differential,  tank  steering  common  to  most  wheeled  robots.  The  passive  torsionally 
compliant  devices  will  be  incorporated  into  enclosures  in  the  individual  Whegs™  hubs. 

The  new  tail  mechanism  is  intended  to  perform  several  functions  for  the  Pelican 
Whegs™.  During  normal  operation,  the  tail  can  be  lowered  to  provide  stability  during 
transit  across  uneven  terrain  to  reduce  undesirable  body  tilt  and  roll.  When  the  robot 
seeks  to  mount  an  obstacle,  the  tail  will  be  lowered  to  provide  additional  motive  force  to 
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help  boost  the  robot.  It  also  replaces  the  body  flexion  function  of  the  DAGSI  Whegs™  by 
acting  as  a  foot  during  climbing,  preventing  the  robot  from  high-centering  and  falling 
back  off  of  obstacles  [4], 


Figure  2.  Pelican  Whegs™  (From  [4]) 

C.  THESIS  CONCEPT 

A  robotic  platform  named  Robster,  seen  in  Figure  3  and  Figure  4,  was  built  to 
emulate  the  basic  functional  design  considerations  and  components  of  the  Pelican 
Whegs™  robot.  This  thesis  is  determined  to  develop  control  and  logic  algorithms  to 
operate  the  tail  mechanism  implementation  in  the  new  Pelican  Whegs™  design  and 
evaluate  the  efficacy  of  the  new  design.  Passive  compliance  devices  were  not  built  for  the 
Robster  due  to  their  complexity  and  poor  reliability  and  are  not  the  focus  of  this  thesis. 
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Figure  3.  ROBSTER,  front  view 


Figure  4.  ROBSTER,  side  view 
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II.  ROBOT  DESIGN 


A.  MECHANICAL  COMPONENTS 

1.  Platform 

The  Robster  chasis  and  platform  components  were  chosen  to  match  the  functional 
capabilities  of  the  Pelican  Whegs™  chasis.  The  four  drive  motors,  motor  controllers, 

motor  battery,  and  tail  assembly  are  mounted  on  a  13"  wide  by  18"  long  thick 

aluminum  plate.  The  tail  assembly  gears,  shafts,  motor,  and  paddle  are  attached  to  a 
separate  removable  platform  connected  to  the  base.  Above  this,  a  separate  aluminum 
plate  is  connected  using  3”  extensions  to  provide  the  base  for  the  electronics.  This  plate 
also  shields  sensitive  electronic  components  from  large  electromagnetic  interference 
generated  during  drive  motor  activation.  The  Robster  weighs  321bs,  primarily  due  to  the 
four  drive  motors  and  the  tail  motor.  This  weight  was  much  greater  than  the  expected 
weight  of  the  Pelican  Whegs™  and  affected  some  of  the  results  of  the  experiment. 

2.  Wheel  Legs 

Robster  has  four  wheel-legs  machined  from  single  pieces  of  hard,  polyvinyl 
chloride  plastic  pictured  in  Figure  5.  Each  Wheg™  consists  of  four,  3.5”  long  spokes 
around  a  central  hub,  evenly  separated  by  90> .  The  four-spoke  leg  has  advantages  and 
disadvantages  compared  to  a  conventional  wheel  that  are  common  to  all  Whegs™  (see 
Figure  6).  In  a  conventional  wheel,  climbing  ability  is  limited  by  the  radius  of  the  wheel, 
no  matter  how  great  the  traction  of  the  wheel.  The  Pelican  Whegs™  can  get  a  foothold 
for  an  obstacle  height  given  by 

h,  =  2[r-cos(^)]  =  2[(3.5")-cos(45°)]  =  5.23"  (1) 

for  a  29.3%  greater  climbing  ability  than  a  wheel.  The  Pelican  Whegs™  also  has  an 
advantage  over  the  previous  DAGSI  Whegs™  model  in  the  vertical  translation  of  its 
Whegs™  hub  during  motion,  which  is  given  by 

h2  =  r[  1  -  cos(f )]  =  (3.5")[l  -  cos(Jf)]  =  0.266"  (2) 
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Vertical  translation  is  only  7.6%  of  hub  height,  which  is  also  43%  less  than  the  13%  of 
hub  height  achieved  by  the  three-spoked  DAGSI  Whegs™.  Consequently,  the  Pelican 
Whegs™  enjoys  a  much  steadier  ride.  This  advantage  helps  to  offset  the  reduced  stability 
of  the  diagonal  vs.  tripod  gait  [1], 


Figure  5.  Robster  Pelican  Whegs™ 
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PELICAN  WHEGS 


Figure  6.  Wheel  vs.  DAGSI  Whegs™  vs.  Pelican  Whegs™ 

3.  Tail  Mechanism 

The  Robster  tail  mechanism,  pictured  in  Figure  7,  was  designed  to  approximate 
the  desired  function  of  the  modeled  Pelican  Whegs™  tail  device.  A  motor  connected  to  a 
steel  rod  running  lengthwise  along  the  center  rear  half  of  the  robot  drives  the  tail.  The 
gears  that  interlink  the  motor  and  shaft  have  a  ratio  of  1 : 1 .  This  shaft,  in  turn,  links  to  a 
second  rod  mounted  parallel  to  the  rear  of  the  platform  with  a  gearing  ratio  of  1:2.  The 
tail  is  a  1 2  "  wide  by  1 2  "  long  by  0.5”  thick  panel  of  clear  PVC  plastic  mounted  to  the 
rear  shaft  by  two  aluminum  brackets.  All  gears  are  straight,  bevel  gears  fabricated  from 
steel. 
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Figure  7.  Tail  Mechanism 
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A  Maxon  RE  40  148866  series  motor  with  a  GP  42C  203123  planetary  gear  head 
is  used  to  drive  the  shaft  of  the  tail  assembly.  This  motor  can  provide  98.7mNm  or 
0.0728  lb-ft  of  continuous  torque  and  operates  at  a  maximum  permissible  speed  of 
8200RPM.  The  Maxon  is  driven  by  a  12VDC  power  source  with  a  max  continuous 
current  of  6A  and  a  no  load  current  of  241mA.  The  planetary  gear  head  provides  a  74:1 
reduction  [6].  The  motor  output  to  the  first  bevel  gear  is  therefore  given  a  maximum  of 
5.387  lb-ft  and  110.8  RPM.  The  output  from  the  motor  to  the  tail  shaft  through  the  1:2 
step  up  gear  is  10.7741b-ft 


Given  the  output  torque  values,  a  tail  height  of  four  inches,  and  a  tail  length  of  12 
inches,  the  vertical  component  of  the  downward  force  generated  by  the  tail  can  be 
calculated  as  follows: 


0=  sin  '|  —  |  =  sin 
r 


y(  Ain 
\12  in 


=  19.5° 


„  T  W.llAlb-ft  in^A„ 

F  =  —  = - —  =  10.774/6 

r  \ft 

Fy  =  Fcos(<9)  =  (l0.774/6)cos(l9.5°)=  10.16/6 


(3) 

(4) 

(5) 


This  would  not  be  sufficient  to  lift  the  whole  robot  body.  However,  it  would  provide  a 
large  additional  positive  vertical  force  during  any  climbing  operation. 


B.  ELECTRICAL  COMPONENTS 
1.  Drive  Control 

a.  Motor  Battery 

The  battery  driving  the  wheel  motors  is  a  rechargeable  24  VDC,  4000 
mAhr  Nickel  Metal  Hydride  (NiMH)  battery  pack,  shown  in  Figure  8.  The  battery  pack  is 
a  2  x  10  array  of  C  cell  batteries  and  weighs  VA  pounds.  It  is  mounted  between  the  two 
sets  of  wheels  and  relatively  centered  in  the  platform  base. 
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b. 


Electronics  Battery 


The  electronics  battery  is  the  Power  Pad  160  rechargeable,  15VDC, 
11,000  mAhr  Lithium  Ion  battery,  shown  in  Figure  9.  This  battery  is  mounted  flat  on 
Robster’s  chassis  and  weighs  2 !4  lbs.  This  battery  can  provide  27.4  minutes  of  power  for 
operation  of  all  Robster  electronic  components. 


Figure  9.  Power  Pad  160  laptop  battery  (From  [5]) 
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c. 


Motor  Controllers 


Two  MD22  Devantech  Dual  Motor  Drivers,  pictured  in  Figure  10,  control 
the  Whegs™  speed  and  direction.  Each  motor  controller  can  handle  5A  current  capacity 
for  motors  from  5  to  50V.  A  3  A  fuse  is  connected  in  line  with  the  +24 V  battery  terminal 
to  prevent  high  current  draw  to  the  circuit  board.  Both  motor  and  logic  ground  are 
internally  connected  through  the  module  providing  the  common  ground  for  the  whole 
robot.  The  motor  controllers  have  five  modes  of  operation.  These  motor  drivers  are  set  to 
Control  Mode  1,  capable  of  providing  two  independent  channels  for  separate  motors,  but 
which  are  instead  tied  together  to  electronically  link  each  motor  pair.  A  DC  analog 
voltage  provides  the  control  signal  from  5V  (Full  Forward)  to  2.5V  (Stop)  to  0.0V  (Full 
Reverse)  [7],  The  analog  voltages  are  provided  by  the  BL2000  through  a  LM6132  Buffer 
circuit.  This  protects  the  BL2000  Digital  to  Analog  Converter  (DAC)  outputs  from  high 
current  draws  that  will  destroy  the  DACs. 


Figure  10.  Devantech  MD-22  motor  controller  (From  [7]) 

2.  Power  Distribution 

Robster  requires  24V,  15V,  12V,  and  5V  to  run  its  various  electronic  and 

mechanical  components.  The  basic  components  for  Robster’ s  power  supply  and 

distribution  system  were  taken  from  the  Bigfoot  robot  developed  by  John  Herkamp  [5], 

Mechanical  power  drawn  from  the  24V  battery  is  routed  directly  to  the  motor  controllers 

driving  the  wheels.  AGC  4A,  250V  glass  fuses  in  line  with  the  motor  controllers  protect 

against  high  current  from  the  batteries.  The  15VDC  laptop  battery  provides  the 
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electronics  power  through  a  separate  bus.  Both  12V  and  5V  are  reduced  from  the  15VDC 
battery  by  means  of  7812  and  7805  voltage  regulators,  respectively.  Common  ground  for 
all  components  is  provided  through  the  wheel  motor  controllers,  which  use  both  5VDC 
and  24VDC  (see  Figure  11).  Voltage  requirements  and  current  loads  for  each  component 
are  delineated  in  Table  1. 

3.  Tail  Control 

The  tail  mechanism  is  driven  by  a  12V  battery  connected  through  a  Devantech 
MD-22  Motor  Controller.  The  battery  is  a  rechargeable  12VDC  NiMH  4000mAh  battery 
pack  comprised  of  a  2  x  5  array  of  C  cell  batteries.  A  4A  fuse  is  place  in  line  with  the 
battery  to  protect  the  motor  controller  from  high  current  draw  from  the  motor.  The  MD- 
22  is  driven  by  a  DAC  on  the  microcontroller,  which  is  protected  by  a  LM6132  Buffer 
circuit.  This  configuration  is  shown  in  Figure  12. 


•■laV  +12V 


Mata 

Uicijr 


bhKlfOMCl 


1 


12V 

5V 

Reg 

1 


:  3XH- 


Figure  1 1 .  Power  Distribution  Design  (From  [5]) 
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Component 

Voltage  Requirements 
(V) 

Current  Requirements 
(mA) 

BL2000 

15 

60 

Router 

12 

160 

Motor  Controllers 

5 

50 

Buffers 

5 

0.36 

PWM 

5 

3 

GPS 

5 

60 

Compass 

5 

35 

Potentiometer 

5 

33 

Total  Current 

401.36 

Battery  Life 

27.4min 

Table  1.  Power  Requirements 


Separate  motor  driver  circuits  were  built  to  drive  a  12V  PWM  Motor  Controller. 
This  motor  driver  configuration  functioned  to  drive  the  tail  mechanism.  However,  this 
arrangement  provided  insufficient  motor  torque  to  lift  the  robot  platform  due  to  voltage 
droop  through  the  voltage  regulator  circuit. 

a.  PWM  Motor  Controller 

The  motor  controller  for  the  tail  mechanism  is  the  SuperDroid  Robots 
PWM  Motor  Controller  (see  Figure  13).  This  motor  controller  can  handle  from  12- 
55VDC  and  drive  3 A  continuously  with  surges  up  to  6A  using  the  LMD18200H-bridge 
IC.  It  has  a  four  pin  header  which  provides  inputs  for  ground,  break,  PWM  input,  and 
direction.  Break  is  used  to  effectively  short  the  Output  terminals  when  set  to  logic  HIGH. 
Direction  controls  the  direction  of  current  flow  between  the  two  Output  leads, 
determining  the  direction  of  motor  rotation.  PWM  input  operates  from  0-5V  at  a 
minimum  of  1kHz.  These  operational  parameters  are  displayed  in  Table  2  [8], 
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Figure  12.  Tail  Component  Functional  Diagram 


A  simple  buffer  circuit  was  built  to  provide  the  signal  for  the  DRIVE  and 
BREAK.  This  buffer  was  built  to  protect  the  digital  output  ports  on  the  BL2000  from 
sudden  high  current  pulls  generated  by  the  motor  controller  using  a  LM6132  Buffer  op- 
amp. 


PWM 

Dir 

Brake 

Active  Output  Drivers 

H 

H 

L 

Source  1,  Sink  2 

H 

L 

L 

Sink  1,  Source  2 

L 

X 

L 

Source  1 ,  Source  2 

H 

H 

H 

Source  1 ,  Source  2 

H 

L 

H 

Sink  1,  Sink  2 

L 

X 

H 

NONE 

Table  2.  Motor  Controller  Truth  Table  (From  [8]) 
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b.  1.4kHz  PWM  Circuit 

A  Pulse  Width  Modulation  (PWM)  circuit  drives  the  motor  controller  (see 
Figure  14).  This  simple  PWM  circuit  is  generated  by  a  LM555  timer  integrated  circuit 
operating  in  an  astable-oscillator  configuration  [9].  From  the  figure  below,  the  resistors 

are  and  ~  with  a  capacitor  ^  i  ”  . 


Figure  13.  PWM  Motor  Controller  (From  [8]) 
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+5V 


Figure  14.  2.56kHz  PWM  Circuit 


Given  the  resistance  and  capacitance  values  for  this  circuit,  the  PWM  circuit  has 
the  following  characteristics  (see  Figure  15): 


/  = 


0.693  +2/?2)C,  0.693(4.7kQ  +  2(470Q))0.1^/F 

Th  =  0.693  (Rj  +  R2)Cx  =  0.693  (4.7&Q  +  470Q)0.1//F  =  358/w 
Tl  =  0.693 =  0.693  (470Q)0.1//F  =  33//s 


=  2.56kHz 


T 

DC  =  H 


358  jus 


Th  +  Tl  358  jus  +  33  jus 


=  91.6% 


(6) 

(7) 

(8) 

(9) 
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Figure  15.  PWM  Waveform  from  Oscilloscope 
c.  Tail  Voltage  Regulator  Circuit 

This  circuit  converts  the  24VDC  power  of  the  motor  battery  and  reduces  it 
to  12VDC  power  for  use  with  the  Maxon  tail  motor,  as  shown  in  Figure  16.  This  circuit 
uses  a  standard  7812  voltage  regulator  IC  with  a  maximum  1A  output  to  provide  maintain 
the  +12V  voltage  [10].  A  high  current  MJ2955  PNP  transistor  and  Dale-RH4Q  ,  25W 
power  resistors  function  to  boost  the  output  current  at  the  regulated  voltage.  A  1A  fuse 
placed  on  the  output  side  of  the  voltage  regulator  prevents  high  current  from  leaking  back 
to  the  regulator. 


d.  Potentiometer 

A  variable  resistance  potentiometer  functioning  as  a  voltage  divider 
determines  inclination  of  the  Tail.  A  Spectrol  536  wire  wound  precision  rotary 
potentiometer  is  used  in  this  application.  The  Spectrol  536  has  resistive  range  from  030 
to  inJf£J  with  a  tolerance  of  .  This  ten  turn  potentiometer  can  be  rotated  through 

.-WKI  [11]. 
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EMITTER 


COLLECTOR 


Figure  16.  12VDC  Voltage  Regulator  circuit 

This  potentiometer  is  firmly  connected  to  the  right  end  of  the  axial  tail  shaft, 
rotating  with  the  tail  shaft,  shown  in  Figure  17.  Five  volts  is  applied  across  the  outer 
terminals  of  the  while  the  wiper  is  connected  to  an  ADC  input  on  the  BL2000.  The 

baseline  voltage  for  this  configuration  2.499V  at  uni  indicating  a  (1  incline.  As  the  tail 
moves  up  or  down,  the  voltage  follows  a  linear  relationship  inversely  proportional  to  the 
change  in  angular  position. 


Figure  17.  Spectral  536  Potentiometer 
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c. 


ELECTRONIC  COMPONENTS 


1.  Microcontroller 

The  BL2000  Wildcat  microcontroller  is  a  single-board  computer  that  offers  high 
performance  in  a  compact  form  factor,  pictured  in  Figure  18.  The  BL2000  incorporates 
the  22.1MHz  Rabbit  microprocessor,  256K  flash  memory,  128K  static  RAM,  28  digital 
input/output  ports,  9  12-bit  analog/digital  converter  inputs,  2  12-bit  digital/analog 
converter  outputs,  4  serial  ports,  and  1  RJ-45  Ethernet  port.  It  is  robust,  highly  adaptable, 
and  easily  programmed  using  Dynamic  C  [12]. 


Figure  18.  BL2000  Microcontroller  (From  [12]) 

2.  Router 

During  operation,  all  communications  with  Robster  are  directed  through  a 
Netgear  Rangemax  240  Wireless  G  router  installed  onboard  the  platform,  pictured  in 
Figure  19.  This  router  operates  on  the  IEEE  standard  802.1  IB  and  G  at  2.4GHz  with  a 
maximum  data  rate  of  240Mbps  and  a  range  of  up  to  300  ft.  It  has  four  built  in  10/100 
Mbps  switch  inputs  to  connect  devices,  one  of  which  is  used  to  connect  the  BL2000.  The 
router  operates  on  12VDC  input  power  and  draws  160mA  of  continuous  current  [13].  All 
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communication  utilizes  the  standard  UPD  protocol.  The  router  is  connected  to  the  RJ-45 
Ethernet  port  on  the  BL2000  and  provides  the  communications  pathway  for  the  JAVA 
interface. 


Figure  19.  Netgear  Rangemax  240  Wireless  Router  (From  [13]) 

3.  GPS 

Positional  data  is  provided  by  the  Garmin  GPS-16HVS  antenna  and  receiver 
system,  shown  in  Figure  20.  The  receiver  is  a  12  channel  Wide  Area  Augmentation 
System  (WAAS)  capable  of  simultaneously  tracking  12  satellites  to  compute  a 
differential  GPS  fix  for  a  position  accuracy  of  3-5m.  It  updates  in  interval  of  1  to  900 
seconds  in  1  second  increments.  This  GPS  unit  requires  3.3  to  6VDC-regulated  power 
typically  drawing  65mA  of  current.  It  communicates  using  true  RS-232  output  and 
asynchronous  serial  input  with  RS-232  and  TTL  voltage  levels.  This  application  uses  the 
National  Marine  Electronics  Association  (NMEA)  0183  v2.0  ASCII  serial  format  with 
GPGGA  as  the  primary  output  sentence  [14]. 
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An  example  GPS  NMEA  output  string  is  as  follows: 

$GPGGA, 123519, 4807.038, N, 01 131.000, E, 1,08, 0.9, 545.4, M, 46.9, M„*47 

GGA  Global  Positioning  System  Fix  Data 

123519  Fix  taken  at  12:35:19  UTC 

4807.038, N  Latitude  48  deg  07.038'  N 

01131 .000,E  Longitude  1 1  deg  3 1 .000'  E 

Fix  quality:  0  =  invalid  1  =  GPS  fix  (SPS) 

2  =  DGPS  fix  3  =  PPS  fix 

4  =  Real  Time  Kinematic  5  =  Float  RTK 

6  =  estimated  (dead  reckoning)  (2.3  feature) 

7  =  Manual  input  mode  8  =  Simulation  mode 

08  Number  of  satellites  being  tracked 

0.9  Horizontal  dilution  of  position 

545.4, M  Altitude,  Meters,  above  mean  sea  level 

46. 9, M  Height  of  geoid  (mean  sea  level)  above  WGS84  ellipsoid 

(empty  field)  time  in  seconds  since  last  DGPS  update 

(empty  field)  DGPS  station  ID  number 

*47  the  checksum  data,  always  begins  with  * 

[14] 


GPS  16 


Figure  20.  Garmin  GPS  16HVS  (From  [14]) 
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4.  Compass 

Robster  heading,  tilt,  and  incline  are  determined  using  the  Honeywell  HMR3000 
Digital  Compass,  shown  in  Figure  21.  The  HMR3000  uses  three  magneto  resistive 
magnetic  sensors  and  a  liquid  filled,  two  axis  tilt  sensor  to  produce  accurate  compensated 
heading  data  for  up  to  45°  of  tilt  (see  Figure  22).  The  magneto-resistive  sensing  elements 
are  composed  of  NiFe  thin  films  deposited  on  a  silicon  substrate  as  a  Wheatstone  resistor 
bridge  (see  Figure  23).  The  magnetometer  has  a  wide  dynamic  range  of  ±2  Gauss 
(200pT)  compared  with  0.65  Gauss  for  earth’s  magnetic  field  and  therefore  should  not 
saturate.  The  compass  has  an  accuracy  of  0.5°  with  0.1°  of  resolution  [15]. 


Figure  21.  HMR3000  Digital  Compass  (From  [15]) 


Metal  Contact 


Applied  Field 


Film 

(NiFe) 


Figure  22.  MR  Sensor  Basics  (From  [15]) 
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Compass  heading  is  calculated  at  13.75Hz  from  5  fdtered  measurements:  TiltX, 
TiltY,  MagX,  MagY,  and  MagZ.  The  HMR3000  is  powered  by  5V  regulated  supply  but 
is  capable  of  operating  with  6- 15V  unregulated  power  supply.  It  communicates  using 
standard  serial  RS-232  connection  using  an  NMEA  0183  output  string  at  19200  baud 
[15]. 

An  example  compass  NMEA  output  string  is  as  follows: 

SPTNTHPR,  85.9,  N, -0.9,  N,  0.8,  N*2C 


HPR 

Heading,  Pitch,  and  Roll 

85.9 

Heading  85.9°  magnetic 

-0.9 

Tilt  -0.9°  x-axis 

0.8 

Roll  0.8°  y-axis 

N*2C 

checksum  for  parity 
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III.  ROBOT  CONTROL 


A.  CONTROL  ALGORITHM 

A  computer  algorithm  embedded  on  the  BL2000  microprocessor  controls  Robster. 
The  code  is  written  in  Dynamic  C  and  compiled  using  Dynamic  C  7.1.9.  The  program 
uses  a  series  of  costatements  to  permit  the  processor  to  conduct  cooperative  multitasking 
operations.  Costatements  are  a  feature  of  Dynamic  C  that  permits  the  program  to  perform 
several  tasks  simultaneously  by  voluntarily  releasing  processor  time  to  the  next  function 
during  delays  in  the  individual  tasks.  The  components  of  this  control  algorithm  were 
developed  for  the  Bender  robot  and  described  in  detail  in  [3].  The  basic  outlines  of  this 
control  algorithm  are  provided  in  Table  3  and  Figure  24. 


FUNCTION 

PORT 

Wheels 

DAC1 

Tail 

DACO 

Compass 

Serial  C 

GPS 

Serial  B 

Potentiometer 

ADCO 

Table  3.  Interface  Architecture 
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Figure  24.  Control  Algorithm  Flow  (After  [3]) 


1.  Manual  Control 

The  user  initiates  the  manual  control  costatement  through  the  JAVA  interface  by 
way  of  port  4001  calls  manual  control.  Manual  control  overrides  all  autonomous 
navigation  functions  and  sets  the  manctrl  flag  that  prevents  the  robot  from  entering  the 
navigation  and  PID  costatements.  Manual  control  receives  a  string  from  the  JAVA 
application  buttons,  converts  the  string  to  integers,  and  parses  it  for  control  voltages  for 
the  left  and  right  motor  pairs.  The  motor  control  signal  for  the  left  motor  pair  is  directed 
by  DAC1  and  the  right  side  by  DACO. 

2.  Waypoint 

The  user  initiates  the  Waypoint  costatement  through  port  4002.  It  stores  the 
waypoint  coordinate  data  from  the  JAVA  Application  and  parses  that  data  into  an 
acceptable  form  for  the  Navigation  costatement.  In  addition,  this  function  also  resets  the 
man_ctrl  flag. 
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3.  Navigation 

The  Navigation  costatement  is  initiated  by  the  Waypoint  costatement  from  the 
JAVA  interface.  It  receives  the  waypoint  data  and  passes  heading  error  and  range  from 
waypoint  information  to  the  Control  costatement.  It  uses  the  error  function  to  determine 
the  heading  error  value  from  the  new_hdg  and  curr  heading  variables. 

4.  GPS 

The  GPS  costatement  triggers  the  GPS  receiver  and  translates  that  data  for  the 
JAVA  GUI  through  port  4004.  The  GPS  receiver  is  controlled  on  the  BL2000  on  Serial 
Port  C.  GPS  data  is  updated  in  the  GUI  every  one  second. 

5.  Compass 

The  compass  costatement  triggers  the  compass  and  translates  the  data  for  heading, 
pitch,  and  roll  to  the  JAVA  GUI  through  port  4003.  The  compass  is  controlled  by  the 
BL2000  on  Serial  Port  B.  The  compass  is  configured  to  update  to  the  BL2000  five  times 
per  second. 

B.  TAIL  CONTROL 

The  tail  costatement  takes  data  from  the  digital  compass  and  potentiometer  inputs, 
determines  the  optimal  angular  position  of  the  tail  mechanism,  and  transmits  the  drive 
signal  to  the  tail  motor.  For  this  experiment,  the  tail  angle  will  follow  the  pitch  angle  of 
the  robot  unless  the  robot  exceeds  a  predetermined  positive  pitch  angle.  At  this  angle,  the 
tail  will  immediately  lower  to  provide  the  boost  function  described  by  [4], 

The  compass  input  is  parsed  from  the  NMEA  string  output  statement  of  the 
compass.  The  ASCII  characters  representing  the  pitch  data  are  stored  in  memory  and 
converted  into  a  floating-point  variable,  which  is  interpreted  by  the  BL2000.  An  offset  is 
added  to  the  Pitch  value  to  correct  for  error  due  to  imperfect  mounting  of  the  compass  to 
the  platform. 

The  tail  position  is  computed  from  the  analog  voltage  generated  by  the 
potentiometer  and  read  into  ADCO.  This  value  is  initialized  upon  startup  to  determine  the 
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voltage  that  corresponds  to  a  0°  angle,  indicating  that  the  tail  is  horizontal.  Moving  the 
tail  updates  the  stored  analog  voltage  and  converts  this  to  a  tail  angle  based  on 


et  =  Cl*{vt-v0) 


(10) 


where  Vo  is  the  initial  voltage  that  corresponds  to  0  =  0° ,  Vt  is  the  current  voltage,  ci  is 
an  experimentally  determined  proportionality  constant  which  converts  the  voltage  to  an 
angular  value  and  0t  is  the  computed  position  of  the  tail  with  respect  to  the  horizontal 
(see  section  IV-A). 

A  proportional  coefficient  determines  the  desired  motor  drive  signal  output  to 
DAC1  (see  Figure  25).  This  statement  provides  a  motor  drive  signal  proportional  to  the 
difference  between  the  current  compass  pitch  and  tail  angle  values  given  by 


(ID 


where  6(is  the  compass  pitch,  Vs  is  the  tail  stop  voltage,  Vt  is  the  tail  motor  drive 
voltage,  and  C2  is  an  experimentally  determined  proportionality  constant  which  converts 
the  angular  measurement  given  in  degrees  into  a  voltage  (see  section  IV-B). 

The  control  algorithm  for  the  tail  is  a  series  of  if  ...then. ..else  function  calls  that 
bound  the  parameters  for  tail  actuation.  The  if... then... else  statement  is  a  C  command 
which  performs  a  task  only  if  defined  criteria  are  met  during  the  function  call.  If  not,  it 
will  check  whether  the  parameters  of  the  else  statement  are  met  before  ignoring  the 
function.  A  series  of  nested  if. ..then. ..else  provide  boundary  conditions  for  different 
motor  commands. 


Compass 

Pitch 


Compensator 


OUTPUT 


6 , 


Tail  Angle 


Figure  25.  Proportional  Control  Loop 


30 


c. 


JAVA  GRAPHICAL  USER  INTERFACE 


Kubilay  Uzan  developed  the  JAVA  Graphical  User  Interface  for  use  in  the  NPS 
SMART  program.  This  program  takes  data  input  from  the  GPS  receiver  and  Compass 
and  returns  motor  control  signals  for  the  wheels.  All  information  is  passed  through  the 
router.  The  user  interface  is  a  JAVA  application,  which  appears  as  a  map  on  the  laptop 
screen  (see  Figure  26).  GPS  data  on  fix  time,  available  satellites,  latitude,  and  longitude, 
and  compass  data  on  heading,  pitch,  and  roll  are  parsed  to  the  interface.  Error  messages 
are  processed  to  a  separate  error  indicator.  Both  manual  control  and  waypoint  navigation 
commands  can  be  input  by  the  user  and  output  to  the  robot  [5]. 


■  HEFOCTC-  (Or  TPQL  NTTTFiKE 


Figure  26.  JAVA  GUI  Screen  Capture  (From  [5]) 
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IV.  RESULTS 


A.  TAIL  POSITION  CALIBRATION 


To  calculate  the  angle  of  the  tail  with  respect  to  the  horizontal,  the  consant,  ci, 
from  Equation  10,  had  to  be  determined  experimentally.  In  other  words,  the 
potentiometer  output  to  the  BL2000  was  characterized  to  generate  an  accurate,  reliable 
measurement  of  the  angle  of  the  tail  as  shown  in  Figure  27. 

To  do  this,  the  robot  was  placed  on  an  elevated  platform  and  the  embedded 
Dynamic  C  program  was  initiated.  The  wheel  and  tail  motors  were  both  deactivated.  The 
tail  was  then  manually  elevated  and  lowered  using  a  Cenco-Lerner  Lab  Jack  to  maintain  a 
fixed  angle.  The  tail  was  moved  in  increments  of  5°  with  each  new  angular  position 
verified  by  a  protractor  and  liquid  level. 

The  result  was  the  linear  relationship  between  the  voltage  and  the  indicated  tail 
angle.  This  data  was  plotted  in  Octave  GNU  and  returned  a  linear  regression  plot  of  the 
resulting  data  pictured  in  Figure  27.  The  slope  of  the  line  generated  is  determined  to  be: 
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Tail  Angle  vs  Voltage 


Figure  27. 


Tail  Angle  vs.  Voltage 
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The  proportionality  constant,  ci,  is  utilized  to  determine  the  actual  tail  angle. 
During  dynamic  evaluation,  the  computed  tail  angle  became  highly  erratic  due  to  noise 
generated  by  other  loads  on  the  source  voltage.  Offsets  in  the  horizontal  base  voltage  also 
developed  due  to  friction  between  the  potentiometer  and  tail  shaft.  To  reduce  these 
effects,  the  proportionality  constant  was  adjusted  several  times  to  test  the  response  of  the 

tail  position  indication.  For  the  proportionality  constant,  a  value  ci  =  500  ^  generated  a 

properly  damped  observed  response.  The  applied  ci  was  only  50%  of  the  determined 
value. 

B.  TAIL  POSITION  VS.  ROBSTER  PITCH  -  STATIC 

Tail  position  response  to  compass  pitch  angle  was  characterized  using  a  static 
demonstration  of  the  tail  control  algorithm.  For  this  experiment,  the  robot  was  placed  on 
an  elevated  platform,  and  the  pitch  of  the  robot  was  manually  manipulated  by  tilting  the 
robot  body,  depicted  in  Figure  28.  The  tail  motor  controls  the  position  of  the  tail  angle  in 
accordance  with  parameters  defined  in  the  robot  control  algorithm. 


a. 


2°  <9  <  2° 


d. 


o<-r 


Figure  28.  Static  Test  Concept 
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The  results  for  the  static  demonstration  were  plotted  in  Figure  29  and  were  in 
agreement  with  the  desired  results.  For  the  plot,  CPU  run  time  starting  at  i  =  200,  the 
robot  pitch  angle  was  steadily  increased  and  the  tail  responded  by  increasing  with  the 
increasing  tail  angle.  When  the  pitch  was  increased  beyond  the  critical  angle,  set  at  15°, 
the  tail  reacted  by  rapidly  changing  its  attitude  downward  to  stop  at  the  negative  stop,  set 
at  -35°  for  this  test.  When  the  pitch  angle  declined  below  the  critical  angle,  the  tail  rapidly 
returned  to  its  position  and  continued  following  the  pitch  angle.  This  event  can  be  seen 
between  i  =  5000  and  i  =  5800.  After  this  event,  the  tail  attitude  continues  to  follow  the 
pitch  angle,  from  a  positive  angle  through  negative  angles  and  back  to  the  horizontal. 


Robot  Pitch  vs.  Tail  Angle 


Figure  29.  Robot  Pitch  and  Tail  Angle 


The  proportionality  constant  C2  from  Equation  11,  used  to  generate  the  output 
control  voltage,  was  determined  analytically  and  through  experimental  testing.  The  limits 
for  the  tail  angle,  0t,  provide  a  maximum  allowable  differential  of  70°.  The  stop  voltage 
for  the  tail  motor  controller  is  Vs=2.5V  with  a  voltage  range  between  0V  for  maximum 
reverse  voltage  and  5V  for  maximum  forward  voltage.  Using  Equation  11,  a  minimum 
value  for  C2  can  be  derived. 
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These  bounds  correspond  to  the  voltage  bandwidth  of  the  motor  controller.  In  the 
case  where  motor  controller  voltage  is  outside  these  bounds,  the  motor  signal  becomes 
erratic  and  cannot  be  properly  characterized.  The  robot  rapidly  approaches  this  limit  in 
the  case  where  pitch  exceeds  1 5°  and  the  tail  moves  down  to  drive  the  rear  of  the  robot. 

To  avoid  this  situation,  C2  =  35  was  chosen  to  provide  a  buffer  for  the  motor 


controller  voltage.  This  limited  maximum  drive  signal  voltage  to  0.5V<VT<4.5V. 

Physical  upper  and  lower  bounds  for  the  tail  angle  were  encountered  where  the 
tail  struck  the  lower  base  of  the  robot  at  its  maximum  negative  angle  and  where  the 
plastic  tail  plank  impacted  the  longitudinal  shaft  of  the  tail  mechanism  at  its  maximum 
positive  angle.  The  physical  limits  of  the  tail  were  measured  to  be  #,max  =  75°  and 


=  -60°.  For  this  test,  the  tail  would  only  actuate  between  limits  -40°  <  6t  <  30°  in 
order  to  avoid  slamming  into  these  stops  and  possibly  damaging  the  assembly.  If  the  tail 
overshoots  these  bounds,  motor  control  voltage  is  set  to  stop  and  then  a  small  drive 
voltage  is  sent  to  reverse  the  direction  of  the  tail  and  return  it  to  the  desired  limits. 


C.  TAIL  POSITION  VS.  ROBSTER  PITCH  -  DYNAMIC 

1.  Experimental  Setup 

Dynamic  tests  were  performed  to  evaluate  the  performance  of  the  tail  algorithm 
during  robot  operation.  This  test  was  used  to  characterize  the  performance  of  the  compass 
pitch  sensor  under  the  non-ideal  conditions  of  yaw  and  vertical  translation  during  robot 
motion.  The  test  characterized  the  response  of  the  tail  control  algorithm  to  this 
unpredictable  environment  and  its  ability  to  generate  a  desired  tail  angle. 

The  dynamic  test  entailed  driving  the  robot  forward  over  a  ramp  elevated  to 
different  slope  angles  and  recording  the  response  of  the  compass  and  potentiometer  with 
time.  The  ramp  was  a  simple  platform  made  of  two  60  "x  40 "  wooden  boards  meeting  at 
vertices  to  form  an  isosceles  triangle,  see  Figure  30.  Wooden  blocks  support  the  apex  of 

36 


this  ramp  from  beneath  and  are  adjusted  to  provide  the  desired  elevation.  Rubber  strips 
were  attached  to  the  surfaces  of  the  Whegs™  to  increase  dynamic  friction  between  the 
plastic  wheels  and  the  smooth  wooden  surface. 


Figure  30.  Dynamic  Test  Ramp 


Figure  31  is  a  pictorial  representation  of  the  desired  dynamic  test  characteristics. 
The  desired  performance  of  the  tail  control,  as  a  function  of  the  robot  pitch  angle,  was 
identical  to  that  of  the  static  test  of  tail  control.  On  level  ground,  the  robot  pitch  should 
indicate  approximately  zero  angle  and  the  tail  should  be  parallel  to  the  surface  (see  Figure 
3 1  a).  As  the  robot  began  to  ascend  the  ramp,  the  pitch  should  indicate  this  increasing 
positive  angle  and  the  tail  angle  should  increase  directly  proportional  to  the  increasing 
pitch  (see  Figure  3 1  b).  As  the  pitch  exceeded  a  critical  value  for  each  ramp  slope,  the  tail 
should  immediately  descend  and  remain  in  that  position  until  the  pitch  fell  below  the 
critical  slope  angle  (see  Figure  31  c).  As  the  robot  descended  the  ramp,  the  pitch  angle 
should  become  negative  and  the  tail  should  continue  to  follow  the  pitch  (see  Figure  31  d). 
As  the  robot  returned  to  level  ground,  the  pitch  angle  and  the  robot  tail  incline  should 
both  indicate  this  by  resuming  the  original  condition  of  zero  angle  for  both  pitch  and  tail 
(see  Figure  31  e). 
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a. 


fi(=0' 


6p=0° 


b. 
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6p=0° 


Figure  3 1 .  Dynamic  Test  Concept 

The  tail  control  algorithm  was  optimized  during  testing  to  generate  a  smoother, 
more  consistent  response  as  the  robot  crossed  the  ramp.  To  record  data,  the  robot 
remained  directly  connected  to  the  laptop  by  means  of  the  BL2000  programming  cable 
and  the  data  was  obtained  by  means  of  a  printf  terminal  output  command.  Output  would 

only  be  collected  for  recorded  changes  of  the  compass  pitch  of  0C  >  |0.2°| .  Ten  data  sets 

were  recorded  utilizing  this  method,  five  for  the  platform  elevated  to  a  10°  slope  and  five 
for  the  platform  elevated  to  a  15°  slope.  An  example  of  an  ideal  data  set  is  depicted  in 
Figure  32.  Letter  labels  in  Figure  32  correspond  to  the  letter  labels  given  to  the  steps 
depicted  in  Figure  3 1 . 
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Hypothetical  Robot  Pitch  and  Tail  Angle 


Figure  32.  Desired  Robot  Pitch  and  Tail  Angle  Plot 


2.  Experimental  Results 

The  data  from  the  experimental  trials  were  plotted  in  MATLAB™  to  evaluate  the 
performance  of  the  pitch  indication  and  tail  sensor.  For  all  plots,  both  individual  data 
points  and  seventh  order  linear  regression  fit  lines  of  each  data  set  are  displayed  with 
different  colors.  The  plots  for  average  robot  pitch  and  tail  angle  for  each  set  of  trials 
includes  error  bars  characterizing  the  standard  deviation  of  these  results  (see  Table  4). 


Trial  (Average) 

Standard  Deviation 

Robot  Pitch  10° 

5.77 

Tail  Angle  10° 

9.41 

Robot  Pitch  15 

7.32 

Tail  Angle  15 

10.06 

Table  4.  Average  Standard  Deviations 
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Large  data  scatter  was  a  significant  factor  in  the  resulting  pitch  and  tail  incline 
data.  Motion  of  the  Whegs™  platform  is  subject  to  a  vertical  translation,  pitch  in  the 
direction  of  motion,  and  yaw  perpendicular  to  the  direction  of  motion  during  normal 
horizontal  movement.  A  Whegs™  platform  is  analogous  to  driving  on  square  wheels. 
Though  the  ends  are  curved  to  the  arc  of  a  circle,  they  cannot  obviate  this  innate 
limitation.  In  addition,  the  compass  mounting  was  jostled  and  vibrated  due  to  these 
shocks.  The  nonlinear  characteristics  of  the  platform  and  the  pitch  sensor  resulted  in  the 
large  scatter  observed  in  the  recorded  data  points  from  the  dynamic  experiment. 

Results  for  the  15°  ramp  were  generally  much  worse  than  the  shallower  ramp  due 
to  mechanical  limitations  of  the  robot.  Climbing  the  steeper  ramp  appeared  to  approach 
the  limits  of  the  torque  capabilities  of  the  driving  motors  and  the  dynamic  friction 
achievable  by  the  Whegs™.  During  each  run,  the  robot  slowed  dramatically  as  it  climbed 
the  ramp,  and  accelerated  rapidly  on  the  downward  side  of  the  ramp.  The  resulting  data 
was  very  chaotic,  particularly  for  the  tail  angle,  even  though  the  robot  appeared  to 
perform  its  desired  tasks  based  on  visual  observations. 

a.  Robot  Pitch 

Figure  33  represents  the  robot  pitch  angle  for  a  single  run  of  the  robot  over 
the  ramp.  The  plot  for  individual  data  points  indicates  a  great  deal  of  noise  in  the  pitch 
indication  as  the  robot  proceeded  over  the  ramp.  However,  the  regression  line  indicates 
that  the  recorded  data  generally  corresponded  with  our  desired  results.  The  large  pitch 
measurement  at  i  =  1600  was  likely  produced  by  the  impact  of  the  front  Whegs™  on  the 
flat  ground  as  the  robot  rapidly  descended  the  slope. 


40 


Robot  Pitch  for  10  deg  slope  -  Single  Trial 


Figure  33.  Robot  Pitch  vs.  Time  -  Data  Points  and  Linear  Fit  for  Single  10°  Trial 

Robot  pitch  data  for  both  ramp  angles  was  plotted  in  Figure  34  and  Figure  35  to 
illuminate  trends  in  the  recorded  pitch  data  over  the  course  of  ten  tests.  From  all  trials,  the 
compass  data  proved  to  be  a  rough  but  useful  estimate  for  characterizing  the  robot’s  pitch 
during  actual  operation.  The  change  in  angle  over  time  is  apparent  through  the  majority 
of  the  regression  plots.  At  a  given  time,  the  pitch  angle  was  observed  to  steadily 
increased  to  a  maximum  in  most  trials.  At  this  point,  a  transition  from  positive  incline  to 
negative  decline  was  observed.  Both  ramp  angles  showed  a  transition  of  pitch  angle  from 
positive  to  negative  as  the  robot  body  continued  along  the  downward  slope.  All  plots 
indicated  the  change  in  slope  of  the  pitch  from  negative  back  toward  zero,  indicating  the 
robot  returning  to  a  level  surface. 

However,  all  trials  produced  a  significant  amount  of  spurious  angular  data  that 
affected  the  response  of  the  tail  control  algorithm.  Both  plots  recorded  large  amounts  of 
scatter  in  the  results.  For  the  shallow  slope,  the  individual  linear  fit  curves  were  fairly 
consistent,  but  all  of  the  runs  significantly  overshot  the  ramp  angle  near  the  tip-over 
point.  On  the  downward  slope  of  the  steeper  ramp,  the  individual  data  points  alternated 
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Robot  Pitch  (deg)  Robot  Pitch  (deg) 


between  positive  and  negative  values  for  several  of  the  runs,  flattening  out  the  regression 
plots.  No  explanation  could  be  found  because  these  results  could  not  be  reproduced 
consistently  through  all  trials  and  were  sometimes  absent  in  others. 


Robot  Pitch  vs  Time  - 10  deg  Slope 


Figure  34.  Robot  Pitch  vs.  Time  -  10° 


Robot  Pitch  vs  Time  - 15  deg  Slope 


Figure  35.  Robot  Pitch  vs.  Time  -  15° 
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The  final  plot,  Figure  36,  compared  average  pitch  results  for  both  ramp  heights. 
The  pitch  results  for  both  trials  produced  average  values  less  than  the  actual  amount,  an 
overdamped  condition.  However,  the  10°  slope  was  much  closer  to  representing  the 
correct  result  than  the  higher  angle  slope.  In  addition,  the  standard  deviation  for  the  15° 
slope  was  27%  larger  than  that  of  the  shorter  slope,  indicating  that  the  results  were  far 
less  consistent  over  the  data  set. 


Average  Robot  Pitch  - 10  deg  and  15  deg  Slope 


Figure  36.  Average  Robot  Pitch  -  10°  and  15° 
b.  Tail  Angle 

Figure  37  represents  the  plot  of  a  single  run  of  the  robot  over  the  ramp.  As 
in  the  plot  of  pitch  data,  the  recorded  tail  angle  data  was  very  noisy,  but  the  regression 
line  generally  corresponded  with  the  desired  results.  The  large  and  rapid  swings  in  tail 
angle  indicate  that  the  tail  response  (see  Equation  11)  could  be  further  damped.  However, 
several  attempts  to  do  this  resulted  in  very  sluggish  tail  response  for  a  variety  of  different 
proportionality  constants. 
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Tail  Angle  for  10  deg  slope  -  Single  Trial 


Figure  37.  Tail  Angle  vs.  Time  -  Data  Points  and  Linear  Fit  for  Single  10°  Trial 

Results  for  the  tail  angle,  plotted  in  Figure  38  and  Figure  39,  were  not  as 
conclusive  as  the  pitch  results.  For  the  majority  of  trials,  the  tail  was  observed  to  perform 
the  desired  characteristics  of  holding  a  fixed  horizontal  position  on  flat  terrain,  holding  a 
positive  angle  during  the  initial  ascent  of  the  positive  slope,  lowering  rapidly  when  pitch 
angle  exceeded  a  set  value,  maintaining  a  negative  angle  on  the  down  slope,  and 
returning  to  a  horizontal  position  following  the  ramp.  This  was  especially  true  of  the 
trials  across  the  10°  slope.  However,  the  regression  plots  showed  that  most  of  the  output 
data  was  not  consistent  for  the  15°  slope,  and  individual  trials  produced  vastly  different 
results. 
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Tail  Angle  vs  Time  - 10  deg  Slope 


Figure  38.  Tail  Angle  vs.  Time  -  10° 


Tail  Angle  vs  Time  - 15  deg  Slope 


Figure  39.  Tail  Angle  vs.  Time  -  15° 
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Comparing  average  results  for  both  ramps  in  Figure  40,  the  15°  slope  produced 
better  results  than  the  shallower  ramp.  This  fact  appears  to  be  more  an  artifact  of  the 
method  for  averaging  the  data,  and  not  representative  of  the  results  of  the  individual  trials 
for  that  set  which  produced  a  wide  variation  in  results  for  each  trial.  However,  the 
standard  deviation  for  the  15°  data  set  was  only  6%  larger  than  that  derived  from  the  10° 
data  set. 


Average  Tail  Angle  - 10  deg  and  15  deg  Slope 


Figure  40.  Average  Tail  Angle  -  10°  and  15° 
c.  Robot  Pitch  vs.  Tail  Angle 

The  final  plot  sets,  Figure  41  and  Figure  42,  compare  average  values  for 
each  trial  set.  This  produced  some  expected  and  some  surprising  results.  Both  plots 
illustrated  the  sudden  change  in  tail  angle  as  the  robot  pitch  exceeded  the  critical  value, 
exactly  the  desired  result.  This  operation  occurred  near  the  maximum  pitch  angles  for 
each  slope.  One  unexpected  result  was  that  the  average  tail  response  for  the  10°  slope  did 
not  reproduce  the  desired  operation  as  well  as  the  15°  slope  for  the  positive  side  of  the 
incline.  During  the  ascent  of  the  15°  slope,  the  robot  slowed  significantly  due  to  the 
greater  torque  requirements  necessary  to  surmount  the  greater  incline,  giving  it  additional 
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time  to  generate  a  response.  However,  both  reacted  correctly  to  angles  in  excess  of  their 
respective  critical  angles,  lowering  the  tail  to  the  boost  position,  and  at  identical  rates.  On 
the  downward  side  of  the  ramp,  the  15°  slope  produced  a  significant  overshoot  of  the 
horizontal  in  comparison  to  the  10°  slope.  This  was  likely  caused  by  the  rapid  and  very 
large  change  in  the  difference  between  the  pitch  and  tail  angle,  producing  a  high  tail 
voltage  in  the  difference  function  (see  Equation  11)  for  a  brief  period.  For  both  the  pitch 
and  tail  angles,  standard  deviation  for  the  1 5°  slope  was  much  larger  than  that  found  in 
the  10°  slope. 
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Robot  Pitch  and  Tail  Angle  - 15  deg  Slope 


Figure  42.  Average  Robot  Pitch  and  Tail  Angle  -  15° 
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V.  RECOMMENDATIONS 


The  purpose  of  this  thesis  was  to  evaluate  sensor  and  control  components  for 
integration  into  future  work  on  the  Pelican  Whegs™  prototype  currently  under 
development  at  Case  Western  Reserve  University.  Robster  proved  that  a  tail  mechanism 
could  be  incorporated  into  the  Pelican  Whegs™  design,  and  that  this  system  could  be 
controlled  using  a  digital  compass  with  tilt  sensor  and  a  variable  resistance  potentiometer 
to  indicate  tail  position.  Robster  could  effectively  keep  track  of  its  pitch  and  maintain  an 
appropriate  attitude  for  its  tail  during  walking  and  climbing  operations.  However, 
performance  was  better  when  climbing  a  shallower  platform  than  a  steeper  one  due  to  the 
underdamped  response  of  the  feedback  loop.  After  evaluating  this  demonstration 
platform,  some  specific  considerations  were  recommended  for  implementation  in  the 
prototype. 

A.  CONTROL  ALGORITHM  IMPROVEMENTS 

Further  work  is  needed  to  develop  the  specific  control  algorithm  to  be 
implemented  in  the  Pelican  Whegs™.  The  primary  improvement  to  the  tail  control 
algorithm  will  need  to  come  in  damping  or  eliminating  response  to  spurious  sensor  data. 
This  can  be  accomplished  by  filtering  sensor  input  data  for  transient  results  that  might  fall 
outside  the  surrounding  data.  A  set  of  data  would  need  to  be  stored  in  memory  and  the 
results  compared  for  outliers.  However,  this  would  take  a  certain  amount  of  computer 
time  and  would  be  highly  reliant  on  the  repetition  frequency  of  the  sensor  queries.  Tail 
reaction  time  to  pitch  changes  would  be  protracted,  but  much  improved  position 
reliability  would  be  obtained. 

A  new  critical  angle  needs  to  be  found  for  the  tail  “boost”  response.  Currently, 
this  angle  was  chosen  to  match  the  parameters  of  the  experimental  setup.  However,  the 
critical  angle  for  this  response  should  occur  when  the  robot’s  center  of  gravity  causes  it  to 
high-center  and  flip  over.  This  angle  would  need  to  be  characterized  specifically  for  the 
Pelican  Whegs™  platform. 
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B.  INCLINOMETER 


A  more  responsive  pitch  indicator  might  be  utilized  to  improve  the  positional 
awareness  of  the  robot.  The  HMR  3000  compass  was  employed  in  this  robot  because  it 
was  already  integrated  into  the  navigation  function  of  the  robot  and  had  a  readily 
available  two-axis  tilt  sensor.  Yet,  inconsistent  pitch  data  during  the  dynamic  tests  was 
the  primary  source  of  error  in  the  results.  A  solid-state  MEMS  inclinometer  would  likely 
provide  better  fidelity  and  a  higher  sampling  rate  than  this.  However,  a  different 
inclinometer  was  not  acquired  due  to  cost  and  lack  of  time  to  implement  in  this  thesis. 

C.  TAIL  ANGLE  SENSOR 

An  optical  shaft  encoder  might  be  used  as  an  alternative  to  the  variable  resistance 
potentiometer.  The  potentiometer  was  very  responsive  to  changes  in  inclination  and 
results  were  easily  interpreted  by  the  BL2000.  However,  the  measurement  was  subject  to 
nonlinear  errors  due  to  noise  in  the  source  voltage.  These  variations  introduced 
unpredictable  results  into  several  of  the  testing  trials.  A  shaft  encoder  would  output  a 
fixed  value  for  each  given  unit  of  rotation.  Therefore,  variations  in  the  input  voltage 
would  not  affect  the  results.  This  might  reduce  some  of  the  inaccurate  results  that  were 
recorded  during  the  dynamic  tests. 
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APPENDIX  A.  TAIL  CONTROL  DYNAMIC  C  CODE 


/* 


Court  ney  Hoi  I  and 

2  9  MAY  2  0  0  9 

ROB  ST  E  R  THESI  S  RESULTS 

Demonstration  of  Walking  and  Tail  Over  Obstacle 

1.  Before  starting,  place  Tail  at  0  deg 

2.  Tail  Control  ■  Turn  ON: 

a.  Compass 

b.  Potentiometer 

c .  Tail 

d.  Mot  or  Controller 

3.  Drives  for  ward  for  XX  seconds 


-*/ 

#def  i  ne  READDELAY  15 

#def i  ne  MAX_ SENTENCE  100 

Imemmap  xmem 
/* . 

Serial  Port  Settings 


#def  i 

ne 

B 1  NBUFSI ZE 

127 

#def  i 

ne 

BOUTBUFSI  ZE 

127 

#def  i 

ne 

Cl  NBUFSI ZE 

127 

#def  i 

ne 

COUTBUF  SI  ZE 

127 

/*--- 

. . . 

Compass  variables 


char  d  i  r  _  s  t  r i  n  g [ 2  ]  ; 
i  nt  st  r  i  n g _ p o s ; 
char  i  nputchar; 
float  cur  r  _  h  d  g ; 

char  compass  sent  ence[ MAX_ SENTENCE] ; 
i  nt  compass_er  r  or ; 

II  Tilt  test  variables 

char  *first,  *second,  *third,  *fourth; 
float  tilt; 
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const  int  compass_del  ay  =  50;  //mi  I  i  -  seconds  to  delay  between  compass 
readings 

const  char  init  str[]  =  "  #BAD=11*7A\  r\ n" ;  II 5  times  per  second 

II  const  char  i  n  f  t  _  s  t  r  [  ]  =  "  #BAD  =  15*7E  \  r  \  n" ;  II  2  0  0  times  per  second 

unsigned  long  compass_wait_time; 

const  int  compass_ti  meout  =  1; 

int  Compass_update; 

/* 


New  PID  Variables 


*/ 


int  c o mpc o n v ; 

const  float  P 

=  1; 

II 

proportional  coefficient 

(concrete) 

const  float  1 

=  5; 

II 

Integral  coefficient 

(concrete) 

const  float  D 

=  3; 

II 

differential  coefficient 

(concrete) 

int  flag; 

II 

deter  mi  nes  left  or  right 

turn  or  stop 

int  f  1  a  g  i  nt ; 

II 

integral  counter 

float  insi  devolts; 

II 

voltage  on  side  to  which 

robot  turns 

float  pScale; 

II 

proportional  scaling  ter 

m 

float  dScale; 

II 

differential  scaling  term 

float  iScale; 

II 

integral  scaling  term 

int  Error; 

II 

heading  error  +/-  180 

int  pr  evEr  r  or ; 

II 

heading  error  previous  sample 

/* 


CTRL  bools 


*/ 


int  ma  n  _  c  t  r  I  ; 
/* . 


Control  Variables 


*/ 


const 

float 

ERR 

_l  NNE R_ 

STOP 

=  90.  0 

track 

stop 

const 

float 

ERR 

_l  NNE R_ 

REV  = 

180.  0 

track 

r  ev 

const 

float 

PW 

STOP  = 

2.  50; 

stop 

c  o  mma  n  d 

const 

float 

PW 

REV  =  1 

.  50; 

results  in 

ma  x 

r  ever  se 

( o  1  d 

4.  00) 

const 

float 

PW 

FWD  =  3 

•  25; 

results  in 

ma  x 

forward 

( o  1  d 

0.  80) 

II  Error(deg)  that  makes  inner 
II  Error(deg)  that  makes  inner 
//Pulse  wi  dt  h  that  results  in 
//Pulse  wi  dt  h  that 
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//Pulse  wi  dt  h  that 


II  wheel  control  for 


float  Leftside,  Right  Side; 


ma  n  u  a  1 

control 

const 

drive 

i  nt 

Mot  or  =1; 

II  wheel 

const 

i  nt 

Tail  =0; 

II  t  ai  1 

const 

i  nt 

r  t  c  h  =0; 

//right 

side 

const 

i  nt 

1  t  _  c  h  =  1; 

II  left 

side 

float 

rot, 

Taie; 

/  /  P  o  t  e  n  t  i  o  me  t  e  r  me  a  s  u  r  e  me  n  t  s 


const  float  T  F  WD  =  3.00; 
const  float  T_  ST  OP  =  2.55; 
const  float  T _ R E V  =  1.00; 
f  I  oat  T_  MOVE ; " 

/* 


Function  Prototypes 


i  nt  c o mpa s s _ g e t _ h d g ( c h a r  sentence! MAXSENTENCE] } ; 
void  msDelay  (long  sd); 
unsigned  long  tO; 

#def i  ne  t  i  me  5 


Mai  n  Function 

******  I 


ma  i  n  (  ) 

{ 

i  nt  i  ,  t ; 

float  diff,  tiltl,  level; 

/* 


Initializations 


br  dl  ni  t  (  ) ; 
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/* 


Mo  tor  Initialization 


anaOutVol  ts(  Motor,  PW_STOP); 
anaOutVol  ts(Tai  I  ,  PW_STOP); 

i  Seal  e=0; 
pScal  e=0; 
dScal  e=0; 

t  i  I  1 1  =  10; 
t  =  0; 
i  =  0; 


/* 


Set  flags 


ma  n  _  c  t  r  I  =  1 ; 
Compass  updat  e  =  0; 


/* 


Initialize  Compass 


ser  Bopen(  9600}  ; 
ser  Bwr  FI  u s  h { ) ; 
ser  Bput  s( i  ni t_st  r ) ; 

rot  =  analnVolts(O); 
ms  Del  a y (  100) ; 
level  =  r  ot ; 

wh  i  I  e  (  1 ) 

{ 

II 


II  Compass  Costatement 

II 

II  this  is  where  we  transmit  the  compass  report  to  the  GUI 
II 


costate 

{ 

wai  tfor  (  Del  ayMs(compass_del  ay)); 
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serBrdFI  ush{  ); 

st  r  i  n g _ p o s  =  0; 

i  nput  char  =  ser  Bget  c( ) ; 

II  find  begining  of  sentence 

c  o mp a  s  s _ wa i  t_t i  me  =  S E C_ T I  ME  R  +  c o mpa s s _ t i  meo u t ; 
//timeout  if  compass  not  working 

whi  I  e  ( i  nput  char  !='$') 

{ 

if  (SEC  TIMER  >  compass_wait_time)  abort; 
i  nput  char  =  ser  Bget  c( ) ; 
msDel  ay ( READDELAY) ; 

} 

II  read  the  sentence 


large 


while  (input  char  !  =  '*'  ) 

{ 

compass_sentence[string_pos]  =  i  nputchar; 
st  r  i  ng_pos++; 

i  f ( st  r i  ng  pos  ==  MAX  SENTENCE) 

st  rf  ngpos  =  0;  II  reset  string  if  too 


i  nput_char  =  ser  Bget  c(  ) ; 
msDel  ay (  READDELAY) ; 

} 

compass_sentence[string_pos]  =  0;  II  add  null 
//Tilt  string  parse 

first  =  s  t  r  t  o  k  ( c  o  mp  a  s  s  _  s  e  n  t  e  n  c  e ,  " , " ) ; 
second  =  strtok( NULL,  " ,  " ) ; 
third  =  st  r  t  ok  (  NULL,  " ,  " ) ; 
fourth  =  st  rt  ok  ( NULL,  " ,  " ) ; 

}  /  /  end  of  compass 


II 

II  Tai  I  Control  Costatement 

II 


costate 

{ 

i  ++; 

tilt  =  at  of  ( f  our  t  h)  +  4.0; 
rot  =  analnVolts(O); 

Tale  =  5  0  0  *  ( r  ot  -  I  evel  ) ; 
diff  =  t  i  I  t  -  Tale; 

T_  MOVE  =  di  f f/45  +  T_  ST  OP ; 

//Move  only  with  tail  angle  between  +30  and  -40 
i  f  (  Tal  e  <=30  &&  Tale  >=  -  40)  { 

if  (tilt  >=-1.0  &&  tilt  <=  1.  0)  { 

II  For  Pitch  within  1  deg  of  0  and  tail  within  5,  STOP 

i  f ( Tal  e  <=  5  &&  Tale  >=  -  5) 

a  n  a  Out  Vo  I  t  s (  Ta  i  I  ,  T_  ST  OP ) ; 
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else  anaOutVol  ts(Tai  I  ,  T  MOVE) ; 

} 

II  For  Pitch  <  15  deg,  Tail  follows  pitch 

if  (tilt  >3.0  &&  tilt  <=  12.  0) 

a  n  a  Out  Vo  I  t  s { Ta i  I  ,  T  MOVE) ; 
II  For  Pitch  >  15  deg,  Tail  moves  DOWN 
if  (tilt  >15.0) 

anaOutVol  ts(Tai  I  ,  T  REV) ; 

II  For  Pitch  <  -3  deg,  Tail  follows  pitch 
if  (tilt  <  -  3.  0) 


} 

else 

anaOutVol  ts(Tai  1  , 

T. 

.MOVE) ; 

i  f 

( Tal  e  >  30) { 

anaOutVol  ts(Tai  1  , 
} 

( Tal  e  <  -  40)  { 

anaOutVol  ts(Tai  1  , 

2. 

3); 

II  Move 

Tai  1 

DOWN  at  stop 

else 

i  f 

2. 

7); 

II  Move 

Tai  1 

UP  at  stop 

} 

//Print  only  for  different  Pitch 

if  ( t  i  I  1 1  >=  t  i  I  t  +0.2  II  t  i  I  1 1  <=  t  i  I  t  -  0.  2)  { 

pr  i  nt  f  (  11  %d  \  t  %.  4f  \  t  %.  4f  \  n " ,  i  ,  tilt,  Tale); 

} 

t  i  I  t 1  =  tilt; 

}  //end  of  tail  state  me  nt 

costate 

{ 

anaOutVol  ts(  Motor,  P  W_  S  T  0  P ) ; 
wai  t  f  or  (  Del  a y Ms {  1  0  0  0  0  ) ) ; 
anaOutVol  ts(  Motor,  PW  FWD) ; 
wai  t  f  or  (  Del  a y Ms (  1  0  0  0  0  ) ) ; 

}  //end  of  mo  tor  drive  state  me  nt 
}/  /  whi  I  e(  1) 

} /  /  ma i  n 

/  J  ^START* **************  **U*N*C*T*I*0N  DESCRIPTION 

compass_get_hdg 

SYNTAX:  i  nt  c o mpa s s _ g e t _ d a t a ( ) ; 

KEYWORDS:  compass  " 

DESCRIPTION:  Parses  a  sentence  to  extract  heading  data. 

This  function  is  able  to  parse  HPR  data  froma 
HMR3000  Digital  Compass 

PARAMETER1:  sentence  -  a  string  containing  a  line  of  HPR  data 
RETURN  VALUE:  0  -  success 

-1  -  parsing  error 

-2  -  heading  marked  invalid 

SEE  ALSO: 

***D*******************************************************  DESCRI  PTI  ON 

i  nt  compass  get  h d g ( char  sentence! MAX  SENTENCE]) 

auto  i  nt  i  ; 

char  *err,*hdg,*type; 

char  error; 

if(strlen(sentence)  <  4) 
return  - 1; 

i  f  ( st  r  ncmp(  sent  ence,  "  $ PTNTHP R" ,  8)  ==  0) 
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{ 

II  parse  hpr  sentence 
type  =  s  t  r  t  o  k ( s  e  n  t  e  n  c  e ,  " ,  " ) ; 
hdg  =  st  rt  ok(  NULL,  11 ,  11 ) ; 
err  =  strtok  ( NULL,  " ,  " ) ; 
i  f ( hdg  ==  NULL) 
return  -  2; 

II  pull  out  data 
cur  r_hdg  =  at  of (  hdg) ; 

error  =  ( i  nt ) er  r ; 
if  ( strncmpf  &error,  "  N" ,  1)  ==  0) 
return  -  2; 

} 

else 

return  - 1; 
return  0; 


void  msDelay  (long  sd) 

{ 

unsigned  long  1 1 ; 
t  1  =  MS _ T I  ME  R; 

for  (  t  1  =  MS  _  T I  ME  R;  MS_TI  ME  R  <  ( sd  +  t 1) 
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APPENDIX  B.  EMBEDDED  DYNAMIC  C  CODE 


LT  Court  ney  Hoi  I  and 

I  .  AGBOT  Code  v  2 .  0  -  18MAR2  0  0  9 
Changes: 

Eliminated  all  references  to  Bigfoot  Arm  and  Thermopile 
Co mme n t  e d  out  sonar 

I  I .  AGBOT  Code  v 2 .  1  -  20MAR2  0  0  9 
Changes: 

Added  accelerometer  costate  me  nt 

III.  ROBSTER  Code  v 2.2-  09APR2009 

Working  Code  for  Co  trims,  GPS,  Compass,  IR,  Navigation 
Changes: 

Deleted  accelerometer  costatement 
Added  Serial  compass 

I  V.  ROBSTER  Code  v 2.3-  13APR2  0  0  9 

Good  working  code  for  all  mechanical  components 
Changes: 

Added  function  for  tail 

Wo r  k i  n g  on  ma  n u a  I  tail  control 

To  Do : 

Navigation  improvements 

V.  ROBSTER  Code  v 2 .  4  -  1 8 MAY  2  0  0  9 
Changes: 

Both  wheel  set  now  on  DAC1  under  Motor 

Tail  now  on  DACO  under  Tail 

Added: 

Potentiometer  for  ADCO  as  Rotation  Sensor 

BL2000  CONNECTI  ONS 
Mot  or  Controllers 

DAC1  <- -  - >  / / 1  ef  t  s i  de  wheel  s 

DACO  < —  >  //right  side  wheel  s 

Tail  Controller 

OUTO  BREAK  BLACK 

OUT  1  FWD/REV  YELLOW 

GPS  Serial  Co mmu n i  c a t  a i  o n s 
TX2  BROWN 

RX2  BROWN  Wl  TH  RED 

GROUND  BLACK 

Compass  Serial  Communi  cat  ai  ons 
TX1  GREY 

RX1  GREY 

GROUND  BLACK 

I  R  Ranger 

*  *  *D  £  *  * **********  f*H  1 1  ********  /  /  J  e  n  t  e  r  * 
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#def  i  ne  RE ADDE LAY  15 
#def i  ne  MAX_ SENTENCE  100 

/* 

Net  wor  k  Set  tings 

*/ 


#def i  ne  MY  IP  ADDRESS 
adr  ess 

#def i  ne  I  NTERFACE  ADDRESS 
address 

#def i  ne  MY  NETMASK 
#def i  ne  MY'GATEWAY 

#def i  ne  MAN  PORT  4001 
#def i  ne  WP  PORT  4002 
#def i  ne  GPS  PORT  4003 
#def i  ne  COMPASS  PORT  4004 
#def i  ne  E R R O R _ PORT  4  0  0  5 

fuse  "dcrtcp. I i  b" 

fmemmap  xmem 

/  * . 

Serial  Port  Set  tings 

*/ 


"  1  9  2.1  6  8.1.2  "  /  /  B  L  2  0  0  0 

"  1  9  2.1  6  8.1.3  "  //Laptop 

"  2  5  5.2  5  5.2  5  5.0  " 

"  1  9  2.  1  6  8.  1.  1  "  II  Router  address 

II  receives  manual  control  data 
II  receives  waypoi  nt  data 
/ /  sends  gps  dat  a 
II  sends  compass  data 
II  sends  error  reports 


#def  i 

ne 

B 1  NBUFSI  ZE 

127 

#def  i 

ne 

BOUT  B  UF  S 1  ZE 

127 

#def  i 

ne 

Cl  NBUFSI  ZE 

127 

#def  i 
/*--- 

ne 

COUTBUFSI  ZE 

127 

GPS  Vari  abl  es 


doubl  e  cur  r_l  at ; 
doubl  e  cur  r_l  on; 

const  int  xmi  tdel  ay  =  100; 

char  sentence! MAX  SENTENCE] ; 
char  di  r _ s t r i  ng[ 2J; 

t  ypedef  struct  { 

int  I  atdegrees; 
int  Ion  degr  ees ; 
double  Fat_mi  nutes; 
double  I  onmi  nutes; 
char  I  atdi  recti  on; 
char  I  o n _ d i  rect i  on; 

}  GPSPos i t i  on; 

GPSPosition  current_pos;  II  Declare  new  GPSPosition  variable 
int  gpserror,  g ps _ er r or _ c ou nt ; 
const  float  pi  =  3.14159; 

const  char  GPS_ Res et [ ] =" $ PGRMI . R\r\n";  II  Reset  Unit 
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II  Clear  all  out  put 
II  Enable  the  GGA 


const  char  GPS  Sent  Cl  r [ ] =" $ PGRMO,  ,  2\ r \  n " ; 
sent  ences 

const  char  GPS  GGA  Enabl  e[ ] =" $ P GRMO,  GPGGA,  1\ r\ n" 
sent  ence 

unsigned  long  gpswai  t_ti  me; 
const  int  gpsti  meout  =  1; 

i  nt  st  r  i  n g _ p o s ; 
char  i  nputchar; 

I* . 

Comp  ass  variables 

*/ 


float  curr  hdg; 

char  compass_sentence[ MAX_ SENTENCE] ; 
int  compass_error; 

//Tilt  test  variables 

char  *  f i  r  s  t ,  *second,  *third,  *fourth; 

float  tilt; 


const  int  compass_delay 
r  eadi  ngs 

const  char  i  ni t  st  r [  ]  = 
II  const  char  inTt_str[] 


=  50;  //mi  I  i  -  seconds 

#B AD  =  1 1  *  7  A\ r \ n " ; 

" #B AD  =  1 5 *  7  E\ r \ n " ; 


to  delay  bet  ween  compass 

II 5  times  per  second 
II  2  0  0  times  per  second 


unsigned  long  compass_wai  t_ti  me; 
const  int  co  mp  a  s  s  _  t  i  meout  =  1; 


int  Compass_update; 


/* 

New  PI  D  Var  i  abl  es 

*/ 


int  compconv; 

const  float  P  =  1;  II 

const  float  I  =  5;  II 

const  f I  oat  D  =  3;  II 

i  nt  f I  ag;  II 

int  f  I  a  g  i  n  t ;  II 

float  insidevolts;  II 

f  I  oat  pScal  e;  II 

f  I  oat  dScal  e;  II 

f  I  oat  i  Seal  e;  II 

i  nt  Er  r  or ;  II 

i  nt  prevError;  II 


proportional  coefficient  (concrete) 
Integral  coefficient  (concrete) 
differential  coefficient  (concrete) 
determines  left  or  right  turn  or  stop 
integral  counter 

voltage  on  side  to  which  robot  turns 
proportional  scaling  term 
differential  scaling  term 
integral  scaling  term 
heading  error  +/ -  180 
heading  error  previous  sample 


/* . 

Co mmu n i  c  a t i  o n s 

*/ 


word  status,  port; 

I  ongwor  d  host ; 

udp  Socket  compassdat  a,  gpsdat  a,  errordata; 
socR  t  ype  wp _ d a t  a ,  ma n _ d a t  a ; 


char  cmdBuf[  2048] ; 
char  c  md  s  t  r  [  20] ,  *  c  md  p  t  r ; 
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char  wpt  Buf [  2048] ; 

char  wptstr[  5  0  0  ],  *wptptr,  *  wp  1 1  mp ; 

char  error_buf[ 200] ; 

/* 

Nav  Variables 

*/ 


const  float  b  r  g  _  e  r  r  o  r  =  5.0;  //Allowable  Bearing  Error 

const  float  r  n  g  _  e  r  r  o  r  =  5.0;  //Allowable  range  error  (in  yards) 


float  I  a  t  _  d  i  f  f ,  I  o  n  _  d  i  f  f ; 
current 


II  The  amount  of  Lat/Long  (in  Seconds  and 
II  Decimal  Seconds  between  Bender's 

II  position  and  the  next  waypoint 


float  theta;  //Angle  (deg)  from  True  North  to  next 

waypoi  nt 

float  hdg  error;  //Angle  (deg)  fromcurrent  heading  to  next 

II  waypoi  nt 


float  new_hdg; 


II  The  Desired  heading  in  degrees 


double  rng,  t  e  mp  _  r  n  g ; 


II  Range  and  temporary  range  (in  yards) 


double  br  g; 


II  Don't  know  what  this  is  for 


I* 

Waypoint  Variables 

*/ 


t  ypedef  struct 

{ 

double  I  at ; 
doubl  e  I  on; 
char  action; 

}  WP ; 

WP  wa y  po i  n t  s [ 1 0 ] ; 

char  passed_waypoi  nt [  10] ; 

waypoi  nt  s 

i  nt  c  u r  r  _  wp; 

char  *  t  e  mp ; 

char  * t  e mp _ I  at,  * t  e mp _ I  on; 
char  *temp_action; 

double  lat,  Ion,  wlat,  wlon; 

/  * 

CTRL  bools 


II  Def i  ne  WP  structure 

II  stores  the  list  of  waypoi  nt  s 

II  Stores  action  value  for  passed 

II  current  wp 


*/ 


i  n  t  ma  n  _  c  t  r  I  ; 
i  nt  GPS  updat  ed; 

/* 

Control  Variables 

*/ 


const  float  E RR_ I  NNE R_ STOP  =  90.0;  //Error(deg)  that  makes  inner  track 
stop 
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const  float  ERRI  NNERREV  =  180.0;  //Error(deg)  that  makes  inner  track 
r  ev 


const  float  P W_ S T 0 P  =  2.50;  //Pulse  width  that  results  in  stop  command 

const  float  PVVREV  =  1.5;  //Pulse  width  that  results  in  max  reverse 

(old  4.  00) 

const  float  P W_ F WD  =  3.5;  //Pulse  width  that  results  in  max 

for  ward  (old  0.80) 


float  Leftside,  RightSide;  II  wheel  control  for  manual  control 

const  i  nt  Mot  or  =  1;  II  wheel  drive 


II  tail  drive 
//right  side 
II  left  side 

//Potentiometer  measurements 
II  Tail  Proportional  equation 

const  float  T  F  WD  =  3.00; 
const  float  T _ S T OP  =  2.55; 
const  float  T'REV  =  1.50; 


const  int  Tail  =  0; 
const  int  r  t  _  c  h  =  0; 
const  int  Itch  =  1; 
float  rot,  Tale,  level; 
f  I  oat  T_  MOVE ; 


I* 

Function  Prototypes 

*/ 


int  compass_get_hdg(  char  sentence! MAXSENTENCE] ) ; 

int  gpsgetposi ti  on(GPSPosi ti  on  *  n  e  wp  o  s ,  char  ^sentence) ; 

int  gps_parse_coordinate(char  *  c  o  o  r  d ,  int  *  degrees,  float  *  minutes); 

int  ERROR  functi on(fl  oat  n  e  w_  h  d  g ) ; 

void  ms  Del  ay  (long  sd) ; 

void  Co  mmSt  a  r  t  ( v  o  i  d ) ; 

unsigned  long  1 0; 

#d ef  i  n e  t  i  me  5 

Main  Function 


ma  i  n  ( ) 

{ 


int  i  ,  t ; 
float  di  f  f ; 


/* 


br  dl  ni  t ( ) ; 

Co  mmSt a  r  t ( ) ; 


Initializations 

*/ 


/* 


Mot  or  Initialization 

*/ 


anaOut Vol  t s ( Mot  or ,  P W_ S T O P ) ; 
a  n  a  Out  Vo  I  t  s  (  Ta  i  I  ,  P  W_  S  T  O  P ) ; 


new_hdg=0; 
i  Seal  e  =0 ; 
pScal  e=0; 
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dScal  e=0; 


/* 

Set  flags 

*/ 


ma  n  _  c  t  r  I  =  1 ; 
GPS_updat  ed  =0; 
Compass  updat  e  =  0; 


/  * . 

Initialize  Compass 

*/ 


ser  B o p e n (  9  6  0  0  ) ; 
s  er  Bwr  F I  us  h ( ) ; 
serBputs(initstr); 


/  * 

Initialize  GPS 

*/ 


serCopen( 9600) ;  II  Open  serial  port  C 

s er Cwr F I  us h ( ) ;  II  Flush  serial  port  C  Buffer 

s er Bput s ( G P S _ Res et ) ;  //  Send  Reset  signal  to  GPS  Receiver 

s er Bput s ( G P S _ Sent _ Cl  r ) ;  II  Send  Clear  signal  to  GPS  Receiver 

serBputsf  GPSGGAEnabl  e) ;  II  Send  GGA  Sentence  enable  signal 

II  (position  info) 

I* . 

Initialize  Tail 

*/ 


rot  =  anal  nVol t  s ( 0) ; 
ms  Del  ay (  100) ; 
level  =  r  ot ; 


II 

II 

II 


wh  i  I  e  (  1 ) 

{ 

tcp_ti  ck( NULL) ; 


Receive  Manual  Control  Data 


costate 

{ 

wai  tfor( sock_recv(  &man_dat  a,  cmdstr,  (  wo r d ) s  i  z eof ( c  mds t r ) ) ) ; 

II  Tokenize  the  string  and  convert  to  integers 
Leftside  =  a  t  o  f  ( s  t  r  t  o  k  ( c  md  s  t  r ,  "  " ) ) ; 

Ri  ghtSi  de  =  at  of ( st rt ok( NULL,  "  / n " ) ) ; 

anaOutVol  ts(  Motor,  RightSide); 

II  a  naOut  Vol  t  s ( Ta i I ,  Leftside); 

if  (  !  ma  n  Ctrl) 

{ 

sprintf(error_buf,  "{Manual  control  data  reci  eved.  .  .  I 
MANUAL  CT RL\ n" ,  cur  r  wp)  ;  " 

sock_puts( &error_data,  error_buf); 

/ / Updat  e  the  flags 
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ma  n  _  c  t  r  I  =  1 ; 


}  //  man  data  costate 


II 

II  Compass  Costatement 

II  this  is  where  we  transmit  the  compass  report  to  the  GUI 
II 


costate 

{ 

waitfor  (  Del  ayMs{  compass_del  ay) ) ; 

ser  Br  d F I  u s h ( ) ; 
stri  ngpos  =  0; 

i  nput_char  =  ser  Bget  c( ) ; 

II  find  begininp  of  sentence 

compass  wa  i  t  _  t  i  me  =  S  E  C_  T I  ME  R  +  compass_ti  meout;  //timeout 
if  compass  not  workfng 

whi I  e  (input  char  !  =  '  $'  ) 

{ 

if  (SEC  TIMER  >  compass_wait_time)  abort; 
inputcFiar  =  s  e  r  B  g  e  t  c  ( ) ; 
ms  Del  ay ( READDELAY)  ; 

} 

II  read  the  sentence 
while  ( i  nputchar  !  =  '*'  ) 

compass_sentence[string_pos]  =  input_char; 
stri  ng_ pos  ++; 

i f ( st  r i  ng  pos  ==  MAX  SENTENCE) 

stri  ngpos  =  0;  II  reset  string  if  too  large 

i  nputchar  =  serBgetcf); 
ms  Del  ay ( READDELAY) ; 

} 

compass_sentence[stri  ng_pos]  =  0;  //add  null 
sock_puts( &compass_data,  compass_sent  ence) ; 

//Tilt  string  parse 

first  =  st  rtok( compass_sent  ence,  " ,  " ) ; 
second  =  strtok(NULL,  " ,  " ) ; 
third  =  st  rt  ok  (NULL,  ",  ")  ; 
fourth  =  st  rt  ok  (NULL,  " ,  " )  ; 

i  f  ( ( c  o  mp  a  s  s  error  =c  o  mp  a  s  s  get  hdg(  compass  sentence))  !  =  0) 

{  "  " 

s  p  r  i  n  t  f  ( e  r  r  o  r  _  b  u  f ,  "{Compass  Error:  %d  \  n " ,  c  o  mp  a  s  s  _  e  r  r  o  r ) ; 
sock  puts(&error  data,  error  buf); 

} 

else 

{ 

Compass  update  =  1; 

} 

}//end  of  compass 


Tail  Control  Costate  me nt 


II 

II 
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II 


costate 

{ 

i  ++; 

tilt  =  at  of  ( f  our  t  h)  +  4.0; 
rot  =  analnVolts(O); 


Tale  =  5  0  0  *  ( r  ot  -  I  evel  ) ; 
diff  =  t  i  I  t  -  Tale; 

T_  MOVE  =  di  f f / 45  +  T_  ST  OP ; 


//Move  only  wi  t  h 
i f ( Tal  e  <=  30  && 
if  (tilt  > 


II  For  Pitch  within  1 
anaOutVol  t  s ( T  a i I , 


II  For  Pitch  <15 


II  For  Pitch  > 


II  For  Pitch  < 


} 

i  f 
deg, 

} 

i  f 
15 

} 

i  f 
-  3 


tail  angle  bet  ween  +3  0  and  -30 
Tale  >=  -  40)  { 

-1.0  &&  tilt  <=  1.  0)  { 
deg  of  0  and  tail  within  5,  STOP 
i f ( Tal  e  <=  5  &&  Tale  >=  -  5) 

T  STOP); 

else  anaOutVol  ts(Tai  I  , 


(tilt  >  3.0  &&  tilt 
Tail  folio ws  pitch 
anaOutVol  t  s  (  T  a  i  I  , 


T_  MOVE ) 
0)  { 
MOVE) ; 


12 


(tilt  >  12. 0)  { 
deg,  Tail  moves  DOWN 

anaOutVol  t  s  (  T  a  i  I  ,  T  _  REV) ; 

(tilt  <  -  3. 0)  { 

deg,  Tail  folio  ws  pitch 

anaOutVol  t  s ( T  a i I ,  T_  MOVE ) 


} 

else  if  ( Tal  e  >  30)  { 

/ / Move  Tail  DOWN  at  stop 

anaOutVol  ts(Tai  I  ,  2.3); 


} 

else  if  (Tale  <  -  40)  { 

/ / Move  Tail  UP  at  stop 

anaOutVol  ts(Tai  I  ,  2.7); 


} 

t i I  t 1  =  tilt; 

}  //end  of  tail  state  me  n  t 


II 

II  Receive  Waypoi  nt  Dat  a 

II 


costate 

{ 

wai  tfor(sock_recv(  &wp_data,  wptstr,  (word)  s i  z eof ( wpt s t r ) ) ) ; 

II  find  begining  of  string 

wptptr  =  wptstr;  //assign  a  pointer 

while  ( *  wp  t  p  t  r  !=  1  $' )  //Step  through  until  begining  of  string 
wpt  p t  r 

wpt  pt  r  ++; 

/  / 1  o  k  e  n  i  z  e 

t  e mp _  I  at  =  s  t  r  t  o k(  wpt  pt  r ,  "  " ) ; 
t  emp'l  on  =  st  rt  ok( NULL,  "  " ) ; 
t  e  mp  _  a  c  t  i  o  n  =  s  t  r  t  o  k  ( N  U  L  L ,  "  " ) ; 
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for  (  i  =0;  i  <10;  i  +  +  ) 

{ 

if  ( ( t  e  mp  _  I  a  t  ==  0  &&  t  e  mp  _  I  on  ==0)  || 
waypoi  nt  s[ i ] .  act i  on  !  =  " P" ) 

{ 

wa y po i  n t s [ i ] . I  a t  =  strtod(temp_l  at,  NULL); 
waypoi  nts[i ]. I  on  =  strtod{temp_l  on,  NULL); 
wa y po i  n t  s [  i  ] .  a c t  i  o n  =  *temp_acti  on; 

}  //End  if  Statement 


t  e mp  I  at  =  st r t  ok( NULL,  "  " ) ; 
temp  Ion  =  strtokf  NULL,  "  "); 
t  emp~act  i  on  =  st  rt  ok{ NULL,  "  " 
}/ / End  for  loop 


cur  r  _  wp  =  0; 

II  Resets  current  WP  to  1st  waypoint.  If  this  is  an 
II  update  to  waypoints,  Nav  will  increment  c  u  r  r  _  wp  until 
/ /  a  good  waypoi  nt  is  there. 

//update  the  flags 
ma  n  _  c  t  r  I  =  0 ; 


%d  \  n " 


spri  ntf(error_buf, 

"  $  WP '  s  recieved.  In  AUTO  NAV  and  preceeding  to  WP 
cur  r  wp) ; 

sock_puts( &error_data,  e  r  r  o  r  _  b  u  f ) ; 

II  these  commands  make  the  robot  start  moving  forward  before 
II  trying  to  find  the  heading  to  avoid  em  surge  near  compass 
a  naOut  Vol  t  s ( r  t  _  c  h,  P W_  F WD) ; 
anaOutVol  ts(  I  t’ch,  PW'FWD); 
ms  Del  ay (  5  0  0  ) ; 


}  /  /  End  Waypoint  Costate  me  nt 


II 

II 

II 


II 


GPS 


costate 

{ 

waitfor  ( Del  aySecf  gps_del  ay) )  ; 

serCrdFI  ush( ) ; 
st  r  i  n g _ p o s  =  0; 
i  nputchar  =  s  er  Cget  c  ( ) ; 

//timeout  if  gps  not  sending  data 

gps  wai  t_ti  me  =  SEC  TI  MER  +  gps_ti  meout; 

whi  I  e  ( i  nput  char  !='$') 

if  ( SECTI  MER  >  g ps _ wa  i  t _ t  i  me )  abort; 
i  n put  _ cfia r  =  s  e r  Cget  c (  ) ; 
msDel  a y { READDELAY)  ; 

} 

II  find  begining  of  sentence 

while  ((input  char  !=  '\r')  &&  (input  char  !='\n')) 

{ 

sent ence[ st r i  ng_pos]  =  i  nputchar; 
string  pos  ++; 

i  f ( st  r i  ng_pos  ==  MAX_ SENTENCE) 
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stri  ngpos  =  0;  II  reset  string  if  too  large 

i  nputchar  =  s  e r  Cget  c ( ) ; 
msDel  ay ( READDELAY)  ; 


sent  ence[ st  r i  ngpos]  =  0; 

sockput  s( &gps_dat  a,  sentence); 

II tcp_t i  c  k { NULL) ; 

g  p  s  _  e  r  r  o  r  =  g ps_ get _ pos i  t i  o n( &c u r r e nt _ pos ,  sentence); 

if  ( ( gps_error  ==  0)  ||  (gps_error  ==  -1)) 
gpserrorcount  =  0; 

else 

{ 

gps  error  count  ++; 

} 

GPS_updat  ed  =1; 

curr  l  at=(current_pos.  I  at  degrees  + 

(current_pos.  I  at_mi  nutes/60)); 

curr_l  on=(current_pos. I  on_degrees  + 

(current_pos.  I  o n _ mi  nutes/  60) ) ; 

II  } 

} 1 1  GPS 

II 

II  Navigation 

II  Passes  heading  error  and  range  to  CTRL  costatement  and  uses  error 
f  unct i  on 

II  to  determine  error  from  new_hdg  and  currheadi  ng 
II  :  : 

costate 

{ 

if  (  ma  n  Ctrl) 

{ 

abort ; 

} 

if  (GPS  updated)  //Navigates  to  new  waypoint 

{ 

/  /  i  f  ( 1)  { 

II  give  fake  lat/long 
/ / cur  r_l  at  =  3  6.  5  9  5; 

/  /  c  u r  r  ~ I  on  =  1  2  1.  8  7  5  3; 

I  at  =  60  *  currl  at;  II  converts  latitude  into 

II  Mi  nutes  and  decimal  mi  nutes 
Ion  =  60  *  c  u  r  r  _ I  on;  II  converts  longitude  into 

II  Minutes  and  decimal  minutes 
wl  at  =  60  *  wa y po i  n t  s  [  c  u r  r  _  wp ] .  I  a t ; 

/ /  Conver  t  s  waypoi  nt  val  ues 

wlon  =  60  *  waypoi  nts[curr_wp] .  I  on;  II  to  decimal  minutes 
II  replaced  by  following  line  for  simplicity 

rng  =sqrt((4000000*(wl  at-l  at ) * ( wl  at-l  at ) ) + 

(  2560000*(  wl  on-l  on) * ( wl  on-l  on))); 
if  (rng  <=  rngerror) 

II  When  close  enough  to  waypoint,  action 
II  code  takes  effect  and  next  waypoint  is  loaded 
{ 

switch  (  way  poi  nt  s  [  c  u  r  r  wp], action) 

{ 
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II  Go  to  next  waypoi  nt 


WP\ n" ) ; 

Foundin'1 ) ; 

/ / Re  I  oads 

WP .  \  n " )  ; 

=  =  0)) 

Foundin'1); 


case  ' T1 

{ 

passed  waypoi  nt  [  cur  r  wp]  =  '  T '  ; 

II  Stores  action  code  in  temp  array 
waypoi  nt  s[ cur r  _  wp ]  .action  =  '  P '  ; 

II  Changes  action  code  to  indicate 
II  WP  has  been  passed 

sock_puts( &error_data,  "$Proceeding  to  next 
cur  r_wp+  +  ; 

while  ( ( waypoi  nt s[ cur r  _  wp ] .  I  at  ==  0)  && 

( waypoi  nt  s[  cur  r  _ wp ] .  I  on  ==  0) ) 

{  //checks  for  valid  WP 
curr  wp  ++; 

if  (curr  wp  ==  10) 

{ 

s oc k_ put s ( &e r r o r _ da t a ,  " $  N  o  Valid  WP 

t  cp  t i  ck( NULL) ; 
ma  n  _  c  t  r  I  =  1 ; 
abort; 

}// End  i  f 
}/ / End  whi I  e 
break; 

}  II  End  case  1 T1 

case  'FI1:  //Start  from  beginning  again 

{ 

for  (  i  =  0;  i  <  10;  i  ++) 
i  or  action  codes 

{ 

waypoi  nt s[ i ].  act  i  on  =  passed  waypoi  n  t  [  i  ] ; 

} 

sock_puts(&error_data, "$Proceedi  ng  back  to  home 
cur  r  _ wp  =  0; 

while  ( ( way poi  nt s [ c u r r  wp ] .  I  at  ==  0)  && 

(  waypoi" nt  s[  cur  r  _ wp ] ,  I  on 

{  //checks  for  valid  WP 

cur  r_wp+  +  ; 

if  (curr  wp  ==  10) 

{ 

sock_puts( &error_data,  " $  N  o  Valid  WP 

t  cp  t i ck( NULL) ; 
ma  n  _  c  t  r  I  =  1 ; 
abort; 

}// End  i  f 
} / / End  wh i I  e 

break; 

}//  End  case  1  H1 

case's1:  II  Stop 

{ 

anaOutVol  ts(rt  ch,  PW  STOP); 

anaOutVol  t  s ( I  t  ch,  PW  STOP) ; 


II  Stops  Bi  gf  oot 


f  or  (  i 
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0;  i  <  10;  i  ++) 


II  Clears  the  Waypoint  array 

{ 


Wa  y  p  o  i  n  t  s 


cl  e a r  e d \  n " } ; 


waypoi  n t  s [ i  ] 
waypoi  n t  s [  i  ] 
waypoi  n t  s [ i  ] 
} / / End  for  loop 


I  at  =  0 
I  on  =  0 
act i  on= 


T1  ; 


s  oc  k_  put  s ( &e r  r  o r  _ da t  a , 

" $ De s t i  n a t i  on 


t  c  p_t i  c  k ( NULL) ; 
ma  n  _  c  t  r  I  =  1 ; 
abort; 

}/ / End  case  1  S' 


Ac h i  eved , 


case  '  C1 :  II  Turn  in  a  circle  then  proceed  to  next 
II  WP?????  this  could  be  a  problem 
{ 

cur  r  _  wp  +  + ; 

while  ( (  waypoi  nt s[ cur r  _  wp ] . I  at  ==  0)  && 

(waypoi  ntsfcurr  wp] .  I  on  ==  0) ) 
{  //checks  for  valid  WP 

cur  r_wp++; 
if  ( cur  r  wp  ==  10) 

{ 

sock_puts(  terror  data,  "  $  N  o  Valid  WP  Foundin'1); 
t  cp  t i  cE( NULL) ; 
ma  n  _  c  t  r  I  =  1 ; 
abort; 

}//  End  i f 
}  /  /  End  wh i I  e 
break; 

}/  / End  case  '  C' 

case  '  P '  :  II  Check  for  passed  waypoints 

cur  r_wp++;  II  Bigfoot  ignores  this  point 
II  and  goes  to  next  one 
while  ( ( way po  i  nt s [ c u r r _ wp] .  I  a t  ==  0)  && 

( waypoi  nt s[ cur r _ wp ] .  I  on  ==  0)) 
{  //  Checks  for  valid  WP 

cur  r_wp++; 
if  ( cur  r  wp  ==  10) 

{ 

s  oc  k  put  s  (  &er  r  or  data,  "  $  N  o  Valid  WP  Foundin'1); 
tcp  ti  cR( NULL) ; 
ma  n  _  c  t  r  I  =  1 ; 
abort ; 

}// End  i f 
} / / End  wh i I  e 
break; 

}/ / End  case  1  P1 


default:  //Indicates  and  invalid  action  code 

{ 

s  p  r  i  n  t  f  ( e  r  r  o  r  _  b  u  f ,  "  $  I  n  v  a  I  i  d  action  for  WP  #  %d  In", 
cur  r_wp) ; 

sock_puts(&error_data,  e  r  r  o  r  _  b  u  f ) ; 
t  cp_t i  c k { NULL) ; 


anaOutVol  ts(  rt  ch,  PW  STOP) ; 
anaOutVol  ts(  I  t  ch,  PW  STOP) ; 
ma  n  _  c  t  r  I  =  1 ;  abort; 

}/ / End  default  case 
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II 

II 

II 


} 1 1  End  Swi  t  c  h 

if  ( c  u  r  r  _  wp  >  9)  II  Should  not  be  here. 

II  Action  for  last  WP  invalid. 


{ 

anaOutVol  ts(rt_ch,  P  W_  S  T  0  P } ; 
a  na  Out  Vo  I  t  s  ( I  t  ’  c  h,  PW'STOP); 
ma  n  Ctrl  =  1 ; 

sockputsC&errordata,  "{Invalid  action  for  wp  9 \ n " ) ; 
tcp  ti  c  k ( NULL); 
abort; 

}/ / End  if  (cur  r  _  wp  >9 ) 

} / / End  if  (rng  <  r  n  g  _  e  r  r  o  r ) 

II  Calculate  new  heading  if  range  not  within  error 

II  3600  converts  lat  diff  and  I  o  n  _  d  i  f  f  to  decimal  seconds 

I  a t  _ d i  f  f  =  3  600  *  [ way poi  nt s [ c u r r _ wp] . I  a t - c u r r _ I  a t ) ; 

Ion  diff  =  3  600  *  (currjon  -  waypoi  nt  s[  cur  r  _  wp  ] .  I  on) ; 

II  deter  mi  ne  theta  in  degrees 

theta  =  a  t  a  n  ( (  I  a  t  _  d  i  f  f )  /  ( I  o  n  _  d i f  f ) )  *  (180  /  pi); 

II  waypoint  located  in  positive  y-axis 
if  ((Ion  diff  ==  0)  &&  ( I  a  t  _  d i f  f  >  0) ) 
new_fidg  =  0; 

II  waypoint  is  located  in  negative  y-axis 
else  if  ( ( I  o  n  _  d  i  f  f  ==  0)  &&  ( I  a  t  _  d  i  f  f  <  0)) 
newhdg  =  180; 

II  waypoint  is  located  in  positive  x-axis 
else  if  ( ( I  o  n  _  d i f  f  >  0)  &&  ( I  a  t  _  d  i  f  f  ==  0)) 
newhdg  =  90; 

II  waypoint  is  located  in  negative  x-axis 
else  if  ( ( I  o  n  _  d i f  f  <  0)  &&  ( I  a  t  _  d  i  f  f  ==  0)) 
newhdg  =  270; 

II  waypoint  is  located  in  the  first  or  fourth  quadrant 
II  (0-90  or  2  7  0  -  0  ) 
else  if  ( I  o  n  _  d  i  f  f  >  0) 

newhdg  =  90  -  (int)ftheta); 

II  waypoint  is  located  in  the  second  or  third  quadrant 
II  (90-  1  8  0  or  1  8  0  -  2  7  0  ) 
else  if  ( I  o  n  _  d  i  f  f  <  0) 

new_hdg  =  270- ( i  nt) (theta) ; 
hdg  error  =  ERROR  f  unct  i  on( new  hdg) ; 
tcp'ti  ck( NULL) ; 

}/  /  End  i f  ( GPS  updat  ed) 

}//  End  NAV  costate 


PI  D  Controls 


costate 

{ 

wa i  t  f  o  r (  !  ma  n  _  c  t  r I  ) ; 

if  ( hdg  error  <=  5.0  &&  hdg  error  >=  -5.0) 

{ 

hdg  er  r  or  =  0.0; 
flag  =  2; 

} 

if  ( ( hdgerror  >=180.0)  || 

((hdg  error  >  -180.0)  &&  (hdg  error  <  0.0))) 

{ 

flag  =  1; 

} 

else 

{ 
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flag  =  0; 

} 

//calculate  proportional  scale  constant 
Error  =  (  i  nt ) ( f  a  b  s ( h  d  g  _  e  r r  or ) ) ; 

if  (Error  >  180) 

{ 

Error  =  360  ■  Error; 

} 

pScal  e  =  ( Error*( 0.  008333) ) ; 

dScale  =  ( (  E r r or - pr ev E r r or ) * (  0 . 0  0  8  3  3  ) ) ; 

i  Seal  e  =  pScal  e  +  i  Seal  e; 

i f ( f I  agi  nt  >40) 

{ 

i  Seal  e  =  0.0; 

} 

f  I  agi  nt  ++; 
pr  evEr  r  or  =  Error; 
i f ( ! ( hdg  error  ==  0.0)) 

C 

insidevolts  =  (P  *  pScale)  +  (I  *  iScale)  +  (D  *  dScale); 

II  Do  not  send  more  than  we  put  out 
i  f ( i  ns i  devol  t  s  >  PW  STOP) 

{ 

insidevolts  =  2.35; 

} 

i f ( f I  ag  ==  0)  II  turn  right 

{ 

a  n  a  Ou  t  Vo  I  t  s  ( r  t  _  c  h ,  insidevolts); 
a  na  Out  Vo  I  t  s (  I  t  c  h,  P W  F  WD) ; 

} 

i f ( f I  ag  ==  1)  II  turn  left 

{ 

anaOutVol  ts(rt  ch,  PW  FWD); 
anaOutVol  ts{  I  t  ch,  insidevolts); 

} 

}//ends  if  for  heading  error  not  =  0 
else 
{ 

II  send  the  right  voltages  to  the  wheels  if  no  heading  error 
II  and  the  range  is  greater  than  the  delta 
anaOutVol  t  s  ( r  t  ch,  PW  FWD); 
anaOutVol  t  s(  I  t'ch,  PW'FWD); 

} 

}//end  PID  costate 
}/  /  whi  I  e(  1) 


} /  /  ma i  n 

i  J  ************** *  ************** *  *  *N*C*T**0N  DESCRIPTION 

compass_get_hdg 
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SYNTAX: 


int  compass  get  dat  a{ ) ; 


KEYWORDS:  compass 

DESCRIPTION:  Parses  a  sentence  to  extract  heading  data. 

This  function  is  able  to  parse  HPR  data  froma 
H  MR  3  0  0  0  Di  g  i  t  a  I  Compass 

PARAMETER1:  sentence  -  a  string  containing  a  line  of  HPR  data 

RETURN  VALUE:  0  -  success 

- 1  -  parsing  error 
-2  -  heading  marked  invalid 

SEE  ALSO: 

*??*******************************************************  DESCRI  PTI  ON 


int  compass  get  h  d  g  ( char  sentence! MAX  SENTENCE]) 

{ 

auto  int  i  ; 

char  *err, *hdg, *type; 

char  error; 

i  ffstrl  en(sentence)  <  4) 
return  -  1; 

i  f(  strncmpf  sentence,  "  $  PT  NT  HP  R" ,  8)  ==  0) 

{ 

II  parse  hpr  sentence 
type  =  strtokf  sentence, 
hdg  =  st  rt  ok(  NULL,  11 ,  11 ) ; 
err  =  strtok  (NULL,  ",  " ) ; 
i f ( hdg  ==  NULL) 
return  -  2 ; 

II  pull  out  data 
cur  r_hdg  =  at  of ( hdg) ; 

error  =(int)err; 
if  ( st  r  nc  mp(  &er  r  or ,  " N" ,  1)  ==  0) 
return  ■ 2 ; 


} 

else 

return  -  1 ; 
return  0; 

} 

/  /  g p s _ p a r se_coor  di  nat  e 

II 

II  Parses  GPS  position  data 
II 

//PARAMETERS:  coord  -  contains  N/S,  E/W 

II  degrees,  minutes  -  positional  information 

II 

//RETURN  VALUE:  0  -  success  (xxxxx.xxxx  minutes) 

II  -1  -  parsing  error 

II 
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nodebug  i  nt  gps_parse_coordi  nate(char  *coord,  i  nt  ^degrees,  float 
*  mi  nut  es ) 

{ 

auto  char  *decimal_point; 
auto  char  t  emp; 
auto  char  *  d  u  mmy ; 

deci  mal  _poi  nt  =  strchrf  coord, 
i  f  (  deci  mal  _  p  o  i  nt  ==  NULL) 
return  -  1; 

temp  =  *(  deci  mal  _poi  nt  -  2); 

*(decimal_point  -  2)  =  0;  //temporary  terminator 
♦degr  ees  =  at  oi ( coor  d) ; 

*(  deci  mal  _poi  nt  -  2)  =  temp;  II  reinstate  character 
♦minutes  =  strtod{  deci  mal  _poi  nt  ■  2,  &dummy); 
return  0; 


i  1  ************** *  *S*T*A*R*T* **************  *F**N*C*T*I*0N  DESCRIPTION 

gps_get_posi t i  on 


SYNTAX:  i  nt  gps_get_posi  ti  onfGPSPosi  ton  *newpos,  char 

♦sent  ence) ; 


KEYWORDS:  gps 


DESCRI  PTI  ON: 
f  ol  I  owi  ng 


Parses  a  sentence  to  extract  position  data. 

This  function  is  able  to  parse  any  of  the 

GPS  sentence  formats:  GGA 


PARAMETER1: 

PARAMETER2: 


newpos  -  a  GPSPosition  structure  to  fill 

sentence  -  a  string  containing  a  line  of  GPS  data 
i  n  NMEA- 0183  f  or  mat 


RETURN  VALUE:  0  -  success 

-1  -  not  differential 

-2  -  sentence  marked  invalid 

-3  -  parsing  error 


SEE  ALSO: 


END 


DESCRI  PTI  ON 


II  can  parse  GGA 

nodebug  i  nt  gps  get  posi t i  on( GPSPosi t i  on  ♦newpos,  char  *sent  ence) 

{  '  ' 
auto  i  nt  i  ; 

if(strlen(sentence)  <  4) 
return  -  3; 

i  f  ( st  r  ncmp(  sent  ence,  "  $ G P G G A " ,  6)  ==  0) 

{ 

II  parse  GGA  sentence 
f  or (  i  =  0;  i  <  11;  i  ++) 

{ 

sentence  =  st  r  chr  ( sent  ence,  1  ,  1  ) ; 
i f ( sent  ence  ==  NULL) 
return  -  3; 
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} 


sent  ence++;  //first  character  in  field 
II  pull  out  data 
i f ( i  ==  1)  //latitude 
{ 

i f (  gps_parse_coordi  nat  e( sentence, 

&ne wpos - >1  a t  _ deg r  ees , 

&newpos  -  >!  at  mT  nut  es ) 

) 

{ 

return  -3;  /  /  get  _  c  oo  r  d  i  na  t  e  failed 

}  } 

i f ( i  ==  2)  II I  at  direction 

{ 

newpos  -  >1  at  direction  =  ^sentence; 

} 

i f ( i  ==  3)  II  longitude 

{ 

i f (  gpspar  secoor  di  nat  e( sentence, 
&newpos- >1  o  n  _  d  e  g  r ees, 

&newpos  -  >1  on  mi  n u t  e s ) 

) 

{ 

return  -  3;  II  get  coordinate  failed 

} 

} 

i  f  ( i  ==  4)  II  Ion  direction 

{ 

newpos  -  >1  on  direction  =  *sentence; 

} 

i f ( i  ==  5)  II  link  quality 
{ 

i f ( * s e n t ence  ==  '  O' ) 
return  -  2 ; 

i f ( * s e n t ence  ==  '  1'  ) 
return  -  1 ; 

} 

} 

} 

else 


{ 

return  -3;  //unknown  sentence  type 

} 

return  0; 


A  ************** *  *SJ*A*R*T* ************** *  *  *  **T*I*0N 

ERROR  f  unct  i  on 


SYNTAX:  i  nt  ERROR  funct i  on( new  hdg) ; 

KEYWORDS:  n  a  v ,  control 

DESCRIPTION:  Determines  heading  error  for  use  by  Nav 

c  o  s  t  a  t  e  me  n  t  s 


PARAMETER1:  new_hdg  -  latest  update  of  bearing  to  next 

di recti  on 

to  drive  based  upon  sonar  contact 
RETURN  VALUE:  hdg_er  r  or 


DESCRI  PTI  ON 

and  Control 
waypoi  nt  or 
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SEE  ALSO: 


END 


i  nt  ERROR  functi on(fl  oat  new  hdg) 

{ 

hdg  error  =  n e w_ h d g  -  curr_hdg; 

if  (hdg  error  <=  6  &&  hdg  error  >=  -6) 
"{ 

hdg  error  =  0; 

}  " 

r  e  t  u  r  n ( h  d  g  error); 

} 


It***************  START  ^  ^  ^  ^  ^  ^  ^  FUNCTION 
gps_get_posi t i  on 


SYNTAX: 
KEYWORDS: 
DESCRI  PTI  ON: 
PARAMETER1: 
SEE  ALSO: 

END 


void  msDel  ay(  I  ong  sd); 
delay,  wait 

introduces  a  defined  ms  delay  loop 
s d  -  n u mbe r  of  ms  to  wait 


DESCRI  PTI  ON 


DESCRI  PTI  ON 


DESCRI  PTI  ON 


void  msDelay  (long  sd) 

{ 

unsigned  long  t 1 ; 
t  1  =  MS  T I  MER; 

for  (t  1"=  MS _ T I  MER;  MS _ T I  MER  <  (sd  +  tl);  ); 


II 


*  *  *  * 


void  CommSt  art  ( ) 

{ 

s  o c  k _  i  ni  t  (  ) ; 

if  ( T( host  =  resol  ve{ I  NTERFACE  ADDRESS))) 

{ 

exi  t (  3) ; 

} 

I* 

open  outgoing  error  port 

*/ 
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if  ( !  udp  open) &er  r  or  data,  ERROR  PORT,  Oxffffffff,  ERROR  PORT 
NULL)) 

{ 

exi  t ( 3) ; 

} 

sock_mode(  &error_data,  TCPMODEASCI I ); 
sock_mode(  &error_data,  U D P _ MO D E _ NOCHK) ; 

/* 

open  incoming  waypoint  port 

*/ 

if  {  !  u  d  p  o  p  e  n ( &wp  data,  WP  PORT,  Oxffffffff,  WP  PORT,  NULL)) 

{ 

sock  puts(&error_data,  " $ U n a b I  e  to  open  WP  UDP  sessi  on\n"); 
e  x  i  t  [  3 )  ; 

} 

s  o c  k _  mo d e (  &wp_  data,  UDP_ MODE_ NOCHK) ; 

/* 

open  incoming  manual  control  port 

*/ 

if  (  !  udp_open(  &  ma  n  _  d  a  t a,  M A  N  _ PORT,  Oxffffffff,  MAN_  PORT,  NULL)) 

sock_puts(&error_data,  " $  U  n  a  b I  e  to  open  MANUAL  UDP  sessi  o  n \ n " ) ; 
ex i  t ( 3 ) ; 

} 

s  o c  k _  mo d e (  & ma  n _  d a  t  a ,  UDP_  MODE_  NOCHK) ; 

/* 

open  outgoing  compass  port 

*/ 

if  (  !  udp  open(  &compass  data,  COMPASS  PORT,  Oxffffffff,  COMPASS  PORT 
NULL))  { 

sock  put s( &er r o r _ d a t a,  " $ U n a b I  e  to  open  COMPASS  UDP  sessi  on\n"); 
ex i  t [ 3 ) ; 

} 

sock_mode(  &compass_dat  a,  TCP_MODE_ASCI  I  ) ; 
sock_mode(  &compass_dat  a,  UDP  MODE  NOCHK) ; 


/* . 

open  outgoing  GPS  port 


if  ( ! udp_open( &g p s _ d a t a,  GP S_ P ORT ,  Oxffffffff,  GPS_PORT,  NULL)) 

sock  puts(&error_data,  " $ U n a b i  e  to  open  GPS  UDP  sessi  on\n"); 
exi  t ( 3) ; 

} 

sock_mode(  &gps_dat  a,  TCP  MODE  ASCI I  ); 
s  o  c  k  ”  mo  d  e  (  &g p s I d a t  a ,  UDP” MODE” NOCHK) ; 

sock_puts( &error_data,  "$Sockets  are  establ i  shed\n"); 

if  (sock  recv  init(  &wp  data,  wptBuf,  ( wo r d ) s i  z eof ( wpt Buf ) ) ) 

{ 

sock  puts(  &error_data,  "  $  C  o  u  I  d  not  enable  WP  bufferin'1); 
exi  t  [3) ; 

} 
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if  (sock  recv  init(  &man  data,  cmdBuf,  ( wo  rd)sizeof(cmdBuf))) 

{ 

sock_puts(  &error_data,  "  $  C  o  u  I  d  not  enable  MAN  bufferin'1); 
ex i  t ( 3 ) ; 

} 

}  II  end  Comm  Start 
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APPENDIX  C.  EXPERIMENTAL  DATA 


Ex  pe 

r  i  me  n  t  1 

Vo  1  t  a  g  e 

Tail  Angle 

Vo  1  t  a  g e 

Tail  Angle 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  344976 

-  4  4.  6  1  9  5  6 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  37968 

-  9.  9  1  5  3  5  2 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  389596 

0 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  349934 

-  3  9.  6  6  1  8  8  4 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  389596 

0 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  389596 

0 

2.  354892 

-  3  4.  7  0  4  2  0  8 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  384638 

-  4.  9  5  7  6  7  6 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  389596 

0 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  394553 

4.  957676 

2.  359849 

-  2  9.  7  4  6  5  3  2 

2.  394553 

4.  957676 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  394553 

4.  957676 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  394553 

4.  9  5  7  6  7  6 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  394553 

4.  957676 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  399512 

9.  915829 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  399512 

9.  915829 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  399512 

9.  915829 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  399512 

9.  915829 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  399512 

9.  915829 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  404469 

14.  873505 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  404469 

14.  873505 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  404469 

14.  873505 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  399512 

9.  915829 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  399512 

9.  915829 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  404469 

14.  873505 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  404469 

14.  873505 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  404469 

14.  873505 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  404469 

14.  873505 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  399512 

9.  915829 
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2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  409427 

19.  831181 

2.  364807 

-  1  9.  8  3  0  7  0  4 

2.  409427 

19.  831181 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  409427 

19.  831181 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  409427 

19.  831181 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  409427 

19.  831181 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  414385 

24.  788857 

2.  359849 

-  2  4.  7  8  8  8  5  7 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  419342 

29.  746532 

2.  364807 

-  2  4.  7  8  8  3  8 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  419342 

29.  746532 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  419342 

29.  746532 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  414385 

24.  788857 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  419342 

29.  746532 

2.  369765 

-  1  9.  8  3  0  7  0  4 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  414385 

24.  788857 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  419342 

29.  746532 

2.  364807 

-  2  4.  7  8  8  3  8 

2.  419342 

29.  746532 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  374723 

-  1  4.  8  7  3  0  2  8 

2.  374723 

-  1  4.  8  7  3  0  2  8 

E  x  p  e  r  i  me  n  t  2 

Ti  me 

Pitch 

Tai  1 

Ti  me 

Pitch 

Tai  1 

1 

0.  7 

0 

6  4  6  8 

9.  3 

9.  9154 

610 

0.  7 

-  4.  9  5  7  9 

6  5  6  9 

8.  4 

12.  3942 

659 

0.  3 

-  4.  9  5  7  9 

6619 

8.  2 

9.  9154 

1212 

1.  1 

-  4.  9  5  7  9 

6  6  7  0 

7 

9.  9154 

1270 

0.  7 

-  2. 4  7  8  8 

6  7  2  1 

7.  5 

9.  9154 

1319 

0.  3 

-  2. 4  7  8  8 

6  7  7  3 

5.  8 

7.  4365 

1369 

0.  7 

-  2. 4  7  8  8 

6  9  2  7 

4.  7 

7.  4365 

1578 

0.  7 

-  2. 4  7  8  8 

6  9  7  9 

3.  6 

4.  9577 

1677 

1.  2 

-  4.  9  5  7  9 

7  0  2  9 

2.  1 

4.  9577 

1794 

2.  3 

-  2. 4  7  8  8 

7146 

0.  5 

2.  4788 

2  0  2  8 

3 

-  2. 4  7  8  8 

7196 

1.  1 

4.  9577 

2  0  8  6 

3.  6 

-  4.  9  5  7  9 

7  3  5  3 

0.  6 

2.  4788 

2188 

4.  7 

4.  9  5  7  7 

7  4  5  5 

-  0.  3 

4.  9577 

2  2  3  9 

4 

4.  9  5  7  7 

7  5  0  5 

0.  8 

4.  9577 

2  2  9  0 

3.  7 

4.  9  5  7  7 

7  6  0  3 

-1.8 

4.  9577 

2  3  9  3 

4.  6 

4.  9  5  7  7 

7  6  6  0 

-  0.  5 

4.  9577 

2  4  9  5 

5.  1 

4.  9  5  7  7 

7  7  0  9 

-  2 

4.  9577 

2  6  5  3 

5.  8 

4.  9  5  7  7 

7  7  6  8 

-  4.  5 

4.  9577 

2  9  0  9 

6.  4 

7.  4  3  6  5 

7  8  2  0 

-  5.  2 

4.  9577 

3011 

6.  9 

4.  9  5  7  7 

7  9  2  3 

-  5.  5 

2.  4788 

3  0  6  2 

7.  3 

7.  4  3  6  5 

7  9  7  3 

-  6.  3 

2.  4788 

3112 

7.  7 

7.  4  3  6  5 

8  0  2  2 

-  7.  3 

2.  4788 

3  3  6  8 

8 

7.  4  3  6  5 

8  0  7  1 

-  8.  4 

0 

3  4  7  2 

8.  6 

4.  9  5  7  7 

8173 

-  9.  2 

0 

3  6  2  4 

9.  1 

7.  4365 

8  2  2  3 

-  8.  7 

-  2.  4  7  8  8 

3  6  7  5 

9.  3 

7.  4365 

8  3  2  3 

-  10.  8 

-  2.  4  7  8  8 

3  7  2  5 

9.  6 

9.  9154 

8  3  7  2 

-11.9 

-  2.  4  7  8  8 

3  7  7  6 

9.  8 

9.  9154 

8  4  2  1 

-  11 

-  2.  4  7  8  8 

3  8  2  7 

10.  1 

9.  9154 

8  4  7  1 

-  10.  8 

-  4.  9  5  7  9 

3  8  7  8 

10.  8 

9.  9154 

8  5  2  0 

-11.6 

-  4.  9  5  7  9 

3  9  2  9 

10.  6 

9.  9154 

8  6  2  1 

-  13.  8 

-  7. 4  3  6  8 

3  9  8  1 

10.  8 

9.  9154 

8  6  7  0 

-  14.  1 

-  4.  9  5  7  9 

4  0  3  2 

11 

9.  9154 

8  7  2  1 

-  14.  5 

-  7.  4  3  6  8 

4137 

11.4 

9.  9154 

8  7  7  0 

-  13.  9 

-  7. 4  3  6  8 

4188 

11.7 

9.  9154 

8  8  2  0 

-  14.  7 

-  9.  9156 

4  2  9  0 

11.  9 

9.  9154 

8  9  2  0 

-  15.  5 

-  9.  9156 

4  3  9  2 

12.  4 

12.  3942 

8  9  6  9 

-  15 

-  9.  9156 

79 


44  4  2 

12.  8 

12.  3942 

9  0  2  0 

-  14.  6 

-  1  2.  3  9  4  4 

4  4  9  2 

13.  1 

12.  3942 

9  0  6  9 

-15.2 

-  1  2.  3  9  4  4 

4  5  4  1 

13.  7 

12.  3942 

9119 

-  14.  9 

-  1  2.  3  9  4  4 

4  5  9  2 

14.  1 

12.  3942 

9219 

-  13.  9 

-  1  4.  8  7  3  3 

4  6  4  2 

13.  8 

12.  3942 

9  2  6  8 

-  13.  7 

-  1  4.  8  7  3  3 

4  7  4  4 

13.  7 

14.  8733 

9319 

-  14.  1 

-  1  4.  8  7  3  3 

4  8  4  7 

13.  9 

14.  8733 

9  4  2  0 

-  13.  7 

-  1  4.  8  7  3  3 

4  8  9  8 

14.  7 

14.  8733 

9  4  6  9 

-  14.  3 

-  1  4.  8  7  3  3 

4  9  9  8 

15,  5 

14.  8733 

9519 

-  14 

-  1  4.  8  7  3  3 

5  0  4  8 

16 

-  1  9.  8  3  0  9 

9  6  2  1 

-  13 

-  1  4.  8  7  3  3 

5  0  9  9 

14.  9 

-  37.  183 

9  6  7  1 

-12.4 

-  1  4.  8  7  3  3 

5151 

17.  2 

-  37.  183 

9  8  2  3 

-  10.  4 

-  1  2.  3  9  4  4 

5  2  0  2 

16.  8 

-  37.  183 

9  8  7  2 

-11.6 

-  1  2.  3  9  4  4 

5  2  5  3 

15.  2 

-  37.  183 

9  9  2  2 

-11.1 

-  9.  9156 

5  3  0  3 

14.  5 

-  37.  183 

1  0  0  2  2 

-  10.  6 

-  9.  9156 

5  3  5  5 

13.  7 

-  3  4.  7  0  4  2 

1  0  2  2  6 

-  9 

-  7. 4  3  6  8 

5  4  0  6 

13.  4 

-  2  9.  7  4  6  5 

1  0  2  7  6 

-  8.  3 

-  9.  9156 

5  4  5  7 

11.7 

7.  4365 

1  0  3  2  7 

-  8.  1 

-  9.  9156 

5  5  0  8 

11.9 

9.  9154 

1  0  3  7  6 

-  8.  3 

-  9.  9156 

5  5  5  8 

12.  6 

9.  9154 

1  0  4  2  7 

-  8.  6 

-  7.  4  3  6  8 

5  6  5  9 

12 

12.  3942 

1  0  4  7  6 

-  8.  9 

-  7.  4  3  6  8 

5710 

12.  2 

12.  3942 

1  0  5  2  7 

-  6.  2 

-  7.  4  3  6  8 

5  7  5  9 

11.1 

12.  3942 

1  0  5  7  6 

-5.8 

-  7.  4  3  6  8 

5  8  0  9 

11.3 

12.  3942 

1  0  6  2  6 

-5.6 

-  4.  9  5  7  9 

5911 

11.1 

12.  3942 

1  0  7  2  8 

-  4 

-  4.  9  5  7  9 

6011 

11.3 

12.  3942 

1  0  7  7  9 

-  3.  8 

-  4.  9  5  7  9 

6214 

10.  3 

14.  8733 

1  0  8  8  2 

-  3.  6 

-  2.  4  7  8  8 

6316 

10.  1 

12.  3942 

1  0  9  3  5 

-1.3 

-  4.  9  5  7  9 

6417 

10.  2 

12.  3942 

11053 

0.  4 

-  4.  9  5  7  9 

E  x  p  e  r  i  me  n  t 

3a  -  10  deg 

Trial  1 

Trial  2 

Ti  me 

Pitch 

Tai  1 

Ti  me 

Pitch 

Tai  1 

1 

-  0.  9 

0 

1 

4 

0 

287 

0.  5 

-  2.  4  7  8  8 

56 

-  0.  5 

0 

335 

-  2 

-  1  4.  8  7  3  3 

152 

-  0.  2 

0 

391 

-  0.  9 

-  1  4.  8  7  3  3 

201 

0.  5 

0 

439 

-  0.  4 

-  1  2.  3  9  4  4 

249 

-  0.  3 

-  2.  4  7  8  8 

534 

-  0.  7 

-  4,  9  5  7  9 

299 

-  0.  5 

2.  4788 

580 

-  0.  4 

-  7.  4  3  6  8 

397 

-1.2 

7.  4365 

676 

0 

-  2.  4  7  8  8 

452 

-  0.  4 

7.  4365 

725 

-  0.  5 

-  4.  9  5  7  9 

747 

-  0.  9 

4.  9577 

867 

3.  7 

-  2.  4  7  8  8 

795 

-  0.  6 

2. 4788 

919 

2 

4.  9577 

938 

20 

-  4.  9  5  7  7 

977 

-  28 

4.  9577 

987 

-  18 

-  1  9.  8  3  0  9 

1025 

-12.9 

-  1  7.  3  5  2  1 

1035 

12.  7 

-  1  4.  8  7  3  3 

1072 

15.  5 

-  1  4.  8  7  3  3 

1085 

9.  9 

-  1  9.  8  3  0  9 

1120 

5.  4 

-  44.  6198 

1135 

21.3 

-  2  9.  7  4  6  5 

1171 

-  7.  2 

-  2  9.  7  4  6  5 

1184 

-  14.  3 

-  1  7.  3  5  2  1 

1218 

3 

-  2  7.  2  6  7  7 

1231 

4.  6 

-  1  4.  8  7  3  3 

1287 

24.  8 

9.  9154 

1281 

27 

-  1  9.  8  3  0  9 

1334 

3 

-  2  2.  3  0  9  8 

1331 

4 

-  2  7.  2  6  7  7 

1404 

25 

-  2  4.  7  8  8  9 

1390 

-  5.  7 

-  1  4.  8  7  3  3 

1453 

7 

-  37.  183 

1439 

4 

-  2.  4  7  8  8 

1504 

3 

-  42.  141 

1498 

27.  7 

4.  9577 

1565 

-12.9 

-  4  9.  5  7  7  5 

1546 

-  1.  6 

-  4.  9  5  7  7 

1614 

-2.8 

-  4  7.  0  9  8  6 

1603 

-  0.  8 

-  4.  9  5  7  7 

1664 

-1.6 

-  3  9.  6  6  1  9 

1649 

-1.7 

-  4.  9  5  7  7 

1715 

-2,5 

-  3  2.  2  2  5  4 

1705 

-  0.  2 

-  4.  9  5  7  7 

1772 

-1.1 

-  2  7.  2  6  7  7 

1752 

0.  2 

-  1  2.  3  9  4  4 

1827 

-  0.  9 

-  1  4.  8  7  3  3 

1800 

-  1 

-  1  2.  3  9  4  4 

1875 

-  0.  3 

2. 4788 

1896 

-  0.  2 

-  9.  9156 

1922 

-1.3 

4.  9577 

1944 

0.  2 

2.  4788 

2  0  3  5 

-  1 

12.  3942 

1993 

0 

2.  4788 

Trial  4 


Tri  al  3 


80 


Ti  me 

Pitch 

Tai  1 

1 

0.  4 

0 

142 

■  0.  7 

9.  9154 

189 

0.  3 

7.  4365 

335 

0.  2 

0 

722 

0.  3 

-  2.  4  7  8  8 

818 

0.  7 

-  2.  4  7  8  8 

865 

4.  8 

4.  9  5  7  7 

914 

18 

7.  4365 

962 

21.1 

-  1  2.  3  9  4  4 

1009 

30.  8 

-  1  9.  8  3  0  9 

1058 

1.  2 

-  1  7.  3  5  2  1 

1113 

23.  7 

-  3  4.  7  0  4  2 

1162 

4 

-  1  7.  3  5  2  1 

1221 

2.  5 

-  1  4.  8  7  3  3 

1276 

14.  2 

-  1  2.  3  9  4  4 

1324 

5.  3 

-  1  9.  8  3  0  9 

1372 

-12.6 

-  1  7.  3  5  2  1 

1420 

-  16 

-  1  9.  8  3  0  9 

1470 

-1.6 

-  1  9.  8  3  0  9 

1526 

-11.6 

-  1  2.  3  9  4  4 

1576 

-1.3 

-  9.  9156 

1689 

-  0.  8 

-  7.  4  3  6  8 

1833 

-  0.  3 

-  4.  9  5  7  7 

1880 

-  0.  6 

0 

2  0  7  5 

-  0.  4 

4.  9  5  7  7 

Trial  5 

Ti  me 

Pitch 

Tai  1 

1 

0.  5 

-  2.  4  7  8  8 

86 

0.  7 

-  4.  9  5  7  7 

134 

1.  1 

-  4.  9  5  7  7 

190 

0.  7 

-  4.  9  5  7  7 

336 

-  0.  8 

-  1  7.  3  5  2  1 

384 

0 

-  4.  9  5  7  7 

434 

-  0.  3 

-  2.  4  7  8  8 

481 

1 

-  4.  9  5  7  7 

530 

0.  7 

-  4.  9  5  7  7 

577 

0.  5 

-  2.  4  7  8  8 

865 

7.  8 

-  4.  9  5  7  7 

915 

24 

7.  4365 

963 

19.  8 

-  1  9.  8  3  0  9 

1011 

18.  4 

-  1  9.  8  3  0  9 

1058 

4 

-  1  7.  3  5  2  1 

1108 

15.  1 

-  2  4.  7  8  8  6 

1155 

23.  6 

-  1  9.  8  3  0  9 

1202 

6.  3 

-  1  4.  8  7  3  3 

1253 

4 

-  1  9.  8  3  0  9 

1376 

-  19.  3 

12.  3944 

1424 

4 

-  7.  4  3  6  5 

1484 

-5.3 

7.  4365 

1534 

-2.7 

4.  9577 

1590 

0.  2 

2.  4788 

1637 

-  0.  4 

2.  4788 

1782 

0 

2.  4788 

Ti  me 

Pitch 

Tai  1 

1 

-  0.  6 

0 

239 

0 

0 

289 

-  0.  7 

0 

435 

-  4.  1 

0 

485 

-  2.  9 

2.  4788 

542 

-1.1 

2.  4788 

888 

4.  1 

-  1  2.  3  9  4  2 

937 

0 

-  7.  4  3  6  5 

986 

2.  7 

-  7.  4  3  6  5 

1042 

3.  7 

14.  8733 

1090 

-  3.  5 

14.  8733 

1139 

-  7.  5 

-  1  9.  8  3  0  9 

1187 

4 

9.  9156 

1370 

35.  3 

4.  9577 

1419 

-1.1 

-  2  4.  7  8  8  6 

1474 

-11.6 

-  1  7.  3  5  2  1 

1521 

-15.7 

-  3  4.  7  0  4  2 

1569 

4 

-  2  9.  7  4  6  3 

1629 

-  7.  1 

-  2  9.  7  4  6  3 

1678 

-  0.  6 

-  3  4.  7  0  4  2 

1725 

0.  2 

-  3  2.  2  2  5  4 

1821 

-  3.  9 

7.  4368 

1871 

-  0.  2 

9.  9156 

1919 

0.  6 

12.  3944 

2164 

0.  3 

14.  8733 

81 


Experiment  3b  -  15  deg 


Tai  1 

T  i  me 

Pitch 

Tai  1 

2.  4788 

1 

-  1.  3 

0 

2.  4788 

169 

-2.1 

-  2  7.  2  6  7  7 

2.  4788 

225 

-  0.  1 

-  2.  4  7  8  8 

2.  4788 

272 

-  1.  8 

-  2.  4  7  8  8 

-  2.  4  7  8  8 

327 

-  1.  3 

-  2.  4  7  8  8 

-  4.  9  5  7  9 

1013 

-  3.  6 

-  7.  4  3  6  8 

-  2.  4  7  8  8 

1062 

7.  4 

-  4.  9  5  7  9 

-  2.  4  7  8  8 

1111 

5.  8 

9.  9154 

-  2.  4  7  8  8 

1161 

-  0.  7 

12.  3942 

-  2.  4  7  8  8 

1209 

24.  2 

2.  4788 

-  7. 4  3  6  8 

1255 

9.  9 

NJ 

4^ 

^1 

OO 

oo 

UD 

-  1  2.  3  9  4  4 

1304 

9.  7 

-  1  7.  3  5  2  1 

12.  3942 

1354 

14.  5 

14.  8733 

17.  3521 

1401 

13.  9 

17.  3521 

17.  3521 

1448 

8.  7 

19.  8309 

14.  8733 

1496 

-  8.  7 

0 

-  1  7.  3  5  2  1 

1546 

24.  2 

-  2.  4  7  8  8 

-  3  4.  7  0  4  2 

1593 

11.4 

-  1  4.  8  7  3  3 

-  3  4.  7  0  4  2 

1641 

-  6.  1 

-  1  4.  8  7  3  3 

-  9.  9156 

1690 

-  9.  7 

-  9.  9156 

0 

1739 

-  8.  9 

-  7.  4  3  6  8 

-  1  2.  3  9  4  4 

1787 

-  10 

-  1  4.  8  7  3  3 

-  1  2.  3  9  4  4 

1837 

-  9.  4 

-  7.  4  3  6  8 

-  9.  9156 

1884 

-  9 

-  7.  4  3  6  8 

-  1  2.  3  9  4  4 

1934 

-  8.  8 

-  7.  4  3  6  8 

-  9.  9156 

1983 

-  9.  6 

0 

-  1  2.  3  9  4  4 

2  0  8  2 

-  9.  7 

-  4.  9  5  7  9 

-  1  2.  3  9  4  4 

Tai  1 

T  i  me 

Pitch 

Tai  1 

0 

1 

3 

0 

0 

66 

-  0.  8 

0 

0 

113 

-  1 

0 

-  4.  9  5  7  9 

162 

-  0.  4 

0 

9.  9154 

209 

-1.3 
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